1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Driver for Xilinx MIPI CSI-2 Rx Subsystem
4  *
5  * Copyright (C) 2016 - 2020 Xilinx, Inc.
6  *
7  * Contacts: Vishal Sagar <vishal.sagar@xilinx.com>
8  *
9  */
10 #include <linux/clk.h>
11 #include <linux/delay.h>
12 #include <linux/gpio/consumer.h>
13 #include <linux/interrupt.h>
14 #include <linux/module.h>
15 #include <linux/mutex.h>
16 #include <linux/of.h>
17 #include <linux/of_irq.h>
18 #include <linux/platform_device.h>
19 #include <linux/v4l2-subdev.h>
20 #include <media/media-entity.h>
21 #include <media/mipi-csi2.h>
22 #include <media/v4l2-common.h>
23 #include <media/v4l2-ctrls.h>
24 #include <media/v4l2-fwnode.h>
25 #include <media/v4l2-subdev.h>
26 #include "xilinx-vip.h"
27 
28 /* Register register map */
29 #define XCSI_CCR_OFFSET		0x00
30 #define XCSI_CCR_SOFTRESET	BIT(1)
31 #define XCSI_CCR_ENABLE		BIT(0)
32 
33 #define XCSI_PCR_OFFSET		0x04
34 #define XCSI_PCR_MAXLANES_MASK	GENMASK(4, 3)
35 #define XCSI_PCR_ACTLANES_MASK	GENMASK(1, 0)
36 
37 #define XCSI_CSR_OFFSET		0x10
38 #define XCSI_CSR_PKTCNT		GENMASK(31, 16)
39 #define XCSI_CSR_SPFIFOFULL	BIT(3)
40 #define XCSI_CSR_SPFIFONE	BIT(2)
41 #define XCSI_CSR_SLBF		BIT(1)
42 #define XCSI_CSR_RIPCD		BIT(0)
43 
44 #define XCSI_GIER_OFFSET	0x20
45 #define XCSI_GIER_GIE		BIT(0)
46 
47 #define XCSI_ISR_OFFSET		0x24
48 #define XCSI_IER_OFFSET		0x28
49 
50 #define XCSI_ISR_FR		BIT(31)
51 #define XCSI_ISR_VCXFE		BIT(30)
52 #define XCSI_ISR_WCC		BIT(22)
53 #define XCSI_ISR_ILC		BIT(21)
54 #define XCSI_ISR_SPFIFOF	BIT(20)
55 #define XCSI_ISR_SPFIFONE	BIT(19)
56 #define XCSI_ISR_SLBF		BIT(18)
57 #define XCSI_ISR_STOP		BIT(17)
58 #define XCSI_ISR_SOTERR		BIT(13)
59 #define XCSI_ISR_SOTSYNCERR	BIT(12)
60 #define XCSI_ISR_ECC2BERR	BIT(11)
61 #define XCSI_ISR_ECC1BERR	BIT(10)
62 #define XCSI_ISR_CRCERR		BIT(9)
63 #define XCSI_ISR_DATAIDERR	BIT(8)
64 #define XCSI_ISR_VC3FSYNCERR	BIT(7)
65 #define XCSI_ISR_VC3FLVLERR	BIT(6)
66 #define XCSI_ISR_VC2FSYNCERR	BIT(5)
67 #define XCSI_ISR_VC2FLVLERR	BIT(4)
68 #define XCSI_ISR_VC1FSYNCERR	BIT(3)
69 #define XCSI_ISR_VC1FLVLERR	BIT(2)
70 #define XCSI_ISR_VC0FSYNCERR	BIT(1)
71 #define XCSI_ISR_VC0FLVLERR	BIT(0)
72 
73 #define XCSI_ISR_ALLINTR_MASK	(0xc07e3fff)
74 
75 /*
76  * Removed VCXFE mask as it doesn't exist in IER
77  * Removed STOP state irq as this will keep driver in irq handler only
78  */
79 #define XCSI_IER_INTR_MASK	(XCSI_ISR_ALLINTR_MASK &\
80 				 ~(XCSI_ISR_STOP | XCSI_ISR_VCXFE))
81 
82 #define XCSI_SPKTR_OFFSET	0x30
83 #define XCSI_SPKTR_DATA		GENMASK(23, 8)
84 #define XCSI_SPKTR_VC		GENMASK(7, 6)
85 #define XCSI_SPKTR_DT		GENMASK(5, 0)
86 #define XCSI_SPKT_FIFO_DEPTH	31
87 
88 #define XCSI_VCXR_OFFSET	0x34
89 #define XCSI_VCXR_VCERR		GENMASK(23, 0)
90 #define XCSI_VCXR_FSYNCERR	BIT(1)
91 #define XCSI_VCXR_FLVLERR	BIT(0)
92 
93 #define XCSI_CLKINFR_OFFSET	0x3C
94 #define XCSI_CLKINFR_STOP	BIT(1)
95 
96 #define XCSI_DLXINFR_OFFSET	0x40
97 #define XCSI_DLXINFR_STOP	BIT(5)
98 #define XCSI_DLXINFR_SOTERR	BIT(1)
99 #define XCSI_DLXINFR_SOTSYNCERR	BIT(0)
100 #define XCSI_MAXDL_COUNT	0x4
101 
102 #define XCSI_VCXINF1R_OFFSET		0x60
103 #define XCSI_VCXINF1R_LINECOUNT		GENMASK(31, 16)
104 #define XCSI_VCXINF1R_LINECOUNT_SHIFT	16
105 #define XCSI_VCXINF1R_BYTECOUNT		GENMASK(15, 0)
106 
107 #define XCSI_VCXINF2R_OFFSET	0x64
108 #define XCSI_VCXINF2R_DT	GENMASK(5, 0)
109 #define XCSI_MAXVCX_COUNT	16
110 
111 /*
112  * Sink pad connected to sensor source pad.
113  * Source pad connected to next module like demosaic.
114  */
115 #define XCSI_MEDIA_PADS		2
116 #define XCSI_DEFAULT_WIDTH	1920
117 #define XCSI_DEFAULT_HEIGHT	1080
118 
119 #define XCSI_VCX_START		4
120 #define XCSI_MAX_VC		4
121 #define XCSI_MAX_VCX		16
122 
123 #define XCSI_NEXTREG_OFFSET	4
124 
125 /* There are 2 events frame sync and frame level error per VC */
126 #define XCSI_VCX_NUM_EVENTS	((XCSI_MAX_VCX - XCSI_MAX_VC) * 2)
127 
128 /**
129  * struct xcsi2rxss_event - Event log structure
130  * @mask: Event mask
131  * @name: Name of the event
132  */
133 struct xcsi2rxss_event {
134 	u32 mask;
135 	const char *name;
136 };
137 
138 static const struct xcsi2rxss_event xcsi2rxss_events[] = {
139 	{ XCSI_ISR_FR, "Frame Received" },
140 	{ XCSI_ISR_VCXFE, "VCX Frame Errors" },
141 	{ XCSI_ISR_WCC, "Word Count Errors" },
142 	{ XCSI_ISR_ILC, "Invalid Lane Count Error" },
143 	{ XCSI_ISR_SPFIFOF, "Short Packet FIFO OverFlow Error" },
144 	{ XCSI_ISR_SPFIFONE, "Short Packet FIFO Not Empty" },
145 	{ XCSI_ISR_SLBF, "Streamline Buffer Full Error" },
146 	{ XCSI_ISR_STOP, "Lane Stop State" },
147 	{ XCSI_ISR_SOTERR, "SOT Error" },
148 	{ XCSI_ISR_SOTSYNCERR, "SOT Sync Error" },
149 	{ XCSI_ISR_ECC2BERR, "2 Bit ECC Unrecoverable Error" },
150 	{ XCSI_ISR_ECC1BERR, "1 Bit ECC Recoverable Error" },
151 	{ XCSI_ISR_CRCERR, "CRC Error" },
152 	{ XCSI_ISR_DATAIDERR, "Data Id Error" },
153 	{ XCSI_ISR_VC3FSYNCERR, "Virtual Channel 3 Frame Sync Error" },
154 	{ XCSI_ISR_VC3FLVLERR, "Virtual Channel 3 Frame Level Error" },
155 	{ XCSI_ISR_VC2FSYNCERR, "Virtual Channel 2 Frame Sync Error" },
156 	{ XCSI_ISR_VC2FLVLERR, "Virtual Channel 2 Frame Level Error" },
157 	{ XCSI_ISR_VC1FSYNCERR, "Virtual Channel 1 Frame Sync Error" },
158 	{ XCSI_ISR_VC1FLVLERR, "Virtual Channel 1 Frame Level Error" },
159 	{ XCSI_ISR_VC0FSYNCERR, "Virtual Channel 0 Frame Sync Error" },
160 	{ XCSI_ISR_VC0FLVLERR, "Virtual Channel 0 Frame Level Error" }
161 };
162 
163 #define XCSI_NUM_EVENTS		ARRAY_SIZE(xcsi2rxss_events)
164 
165 /*
166  * This table provides a mapping between CSI-2 Data type
167  * and media bus formats
168  */
169 static const u32 xcsi2dt_mbus_lut[][2] = {
170 	{ MIPI_CSI2_DT_YUV422_8B, MEDIA_BUS_FMT_UYVY8_1X16 },
171 	{ MIPI_CSI2_DT_YUV422_10B, MEDIA_BUS_FMT_UYVY10_1X20 },
172 	{ MIPI_CSI2_DT_RGB444, 0 },
173 	{ MIPI_CSI2_DT_RGB555, 0 },
174 	{ MIPI_CSI2_DT_RGB565, 0 },
175 	{ MIPI_CSI2_DT_RGB666, 0 },
176 	{ MIPI_CSI2_DT_RGB888, MEDIA_BUS_FMT_RBG888_1X24 },
177 	{ MIPI_CSI2_DT_RAW6, 0 },
178 	{ MIPI_CSI2_DT_RAW7, 0 },
179 	{ MIPI_CSI2_DT_RAW8, MEDIA_BUS_FMT_SRGGB8_1X8 },
180 	{ MIPI_CSI2_DT_RAW8, MEDIA_BUS_FMT_SBGGR8_1X8 },
181 	{ MIPI_CSI2_DT_RAW8, MEDIA_BUS_FMT_SGBRG8_1X8 },
182 	{ MIPI_CSI2_DT_RAW8, MEDIA_BUS_FMT_SGRBG8_1X8 },
183 	{ MIPI_CSI2_DT_RAW10, MEDIA_BUS_FMT_SRGGB10_1X10 },
184 	{ MIPI_CSI2_DT_RAW10, MEDIA_BUS_FMT_SBGGR10_1X10 },
185 	{ MIPI_CSI2_DT_RAW10, MEDIA_BUS_FMT_SGBRG10_1X10 },
186 	{ MIPI_CSI2_DT_RAW10, MEDIA_BUS_FMT_SGRBG10_1X10 },
187 	{ MIPI_CSI2_DT_RAW12, MEDIA_BUS_FMT_SRGGB12_1X12 },
188 	{ MIPI_CSI2_DT_RAW12, MEDIA_BUS_FMT_SBGGR12_1X12 },
189 	{ MIPI_CSI2_DT_RAW12, MEDIA_BUS_FMT_SGBRG12_1X12 },
190 	{ MIPI_CSI2_DT_RAW12, MEDIA_BUS_FMT_SGRBG12_1X12 },
191 	{ MIPI_CSI2_DT_RAW12, MEDIA_BUS_FMT_Y12_1X12 },
192 	{ MIPI_CSI2_DT_RAW16, MEDIA_BUS_FMT_SRGGB16_1X16 },
193 	{ MIPI_CSI2_DT_RAW16, MEDIA_BUS_FMT_SBGGR16_1X16 },
194 	{ MIPI_CSI2_DT_RAW16, MEDIA_BUS_FMT_SGBRG16_1X16 },
195 	{ MIPI_CSI2_DT_RAW16, MEDIA_BUS_FMT_SGRBG16_1X16 },
196 	{ MIPI_CSI2_DT_RAW20, 0 },
197 };
198 
199 /**
200  * struct xcsi2rxss_state - CSI-2 Rx Subsystem device structure
201  * @subdev: The v4l2 subdev structure
202  * @format: Active V4L2 formats on each pad
203  * @default_format: Default V4L2 format
204  * @events: counter for events
205  * @vcx_events: counter for vcx_events
206  * @dev: Platform structure
207  * @rsubdev: Remote subdev connected to sink pad
208  * @rst_gpio: reset to video_aresetn
209  * @clks: array of clocks
210  * @iomem: Base address of subsystem
211  * @max_num_lanes: Maximum number of lanes present
212  * @datatype: Data type filter
213  * @lock: mutex for accessing this structure
214  * @pads: media pads
215  * @streaming: Flag for storing streaming state
216  * @enable_active_lanes: If number of active lanes can be modified
217  * @en_vcx: If more than 4 VC are enabled
218  *
219  * This structure contains the device driver related parameters
220  */
221 struct xcsi2rxss_state {
222 	struct v4l2_subdev subdev;
223 	struct v4l2_mbus_framefmt format;
224 	struct v4l2_mbus_framefmt default_format;
225 	u32 events[XCSI_NUM_EVENTS];
226 	u32 vcx_events[XCSI_VCX_NUM_EVENTS];
227 	struct device *dev;
228 	struct v4l2_subdev *rsubdev;
229 	struct gpio_desc *rst_gpio;
230 	struct clk_bulk_data *clks;
231 	void __iomem *iomem;
232 	u32 max_num_lanes;
233 	u32 datatype;
234 	/* used to protect access to this struct */
235 	struct mutex lock;
236 	struct media_pad pads[XCSI_MEDIA_PADS];
237 	bool streaming;
238 	bool enable_active_lanes;
239 	bool en_vcx;
240 };
241 
242 static const struct clk_bulk_data xcsi2rxss_clks[] = {
243 	{ .id = "lite_aclk" },
244 	{ .id = "video_aclk" },
245 };
246 
247 static inline struct xcsi2rxss_state *
to_xcsi2rxssstate(struct v4l2_subdev * subdev)248 to_xcsi2rxssstate(struct v4l2_subdev *subdev)
249 {
250 	return container_of(subdev, struct xcsi2rxss_state, subdev);
251 }
252 
253 /*
254  * Register related operations
255  */
xcsi2rxss_read(struct xcsi2rxss_state * xcsi2rxss,u32 addr)256 static inline u32 xcsi2rxss_read(struct xcsi2rxss_state *xcsi2rxss, u32 addr)
257 {
258 	return ioread32(xcsi2rxss->iomem + addr);
259 }
260 
xcsi2rxss_write(struct xcsi2rxss_state * xcsi2rxss,u32 addr,u32 value)261 static inline void xcsi2rxss_write(struct xcsi2rxss_state *xcsi2rxss, u32 addr,
262 				   u32 value)
263 {
264 	iowrite32(value, xcsi2rxss->iomem + addr);
265 }
266 
xcsi2rxss_clr(struct xcsi2rxss_state * xcsi2rxss,u32 addr,u32 clr)267 static inline void xcsi2rxss_clr(struct xcsi2rxss_state *xcsi2rxss, u32 addr,
268 				 u32 clr)
269 {
270 	xcsi2rxss_write(xcsi2rxss, addr,
271 			xcsi2rxss_read(xcsi2rxss, addr) & ~clr);
272 }
273 
xcsi2rxss_set(struct xcsi2rxss_state * xcsi2rxss,u32 addr,u32 set)274 static inline void xcsi2rxss_set(struct xcsi2rxss_state *xcsi2rxss, u32 addr,
275 				 u32 set)
276 {
277 	xcsi2rxss_write(xcsi2rxss, addr, xcsi2rxss_read(xcsi2rxss, addr) | set);
278 }
279 
280 /*
281  * This function returns the nth mbus for a data type.
282  * In case of error, mbus code returned is 0.
283  */
xcsi2rxss_get_nth_mbus(u32 dt,u32 n)284 static u32 xcsi2rxss_get_nth_mbus(u32 dt, u32 n)
285 {
286 	unsigned int i;
287 
288 	for (i = 0; i < ARRAY_SIZE(xcsi2dt_mbus_lut); i++) {
289 		if (xcsi2dt_mbus_lut[i][0] == dt) {
290 			if (n-- == 0)
291 				return xcsi2dt_mbus_lut[i][1];
292 		}
293 	}
294 
295 	return 0;
296 }
297 
298 /* This returns the data type for a media bus format else 0 */
xcsi2rxss_get_dt(u32 mbus)299 static u32 xcsi2rxss_get_dt(u32 mbus)
300 {
301 	unsigned int i;
302 
303 	for (i = 0; i < ARRAY_SIZE(xcsi2dt_mbus_lut); i++) {
304 		if (xcsi2dt_mbus_lut[i][1] == mbus)
305 			return xcsi2dt_mbus_lut[i][0];
306 	}
307 
308 	return 0;
309 }
310 
311 /**
312  * xcsi2rxss_soft_reset - Does a soft reset of the MIPI CSI-2 Rx Subsystem
313  * @state: Xilinx CSI-2 Rx Subsystem structure pointer
314  *
315  * Core takes less than 100 video clock cycles to reset.
316  * So a larger timeout value is chosen for margin.
317  *
318  * Return: 0 - on success OR -ETIME if reset times out
319  */
xcsi2rxss_soft_reset(struct xcsi2rxss_state * state)320 static int xcsi2rxss_soft_reset(struct xcsi2rxss_state *state)
321 {
322 	u32 timeout = 1000; /* us */
323 
324 	xcsi2rxss_set(state, XCSI_CCR_OFFSET, XCSI_CCR_SOFTRESET);
325 
326 	while (xcsi2rxss_read(state, XCSI_CSR_OFFSET) & XCSI_CSR_RIPCD) {
327 		if (timeout == 0) {
328 			dev_err(state->dev, "soft reset timed out!\n");
329 			return -ETIME;
330 		}
331 
332 		timeout--;
333 		udelay(1);
334 	}
335 
336 	xcsi2rxss_clr(state, XCSI_CCR_OFFSET, XCSI_CCR_SOFTRESET);
337 	return 0;
338 }
339 
xcsi2rxss_hard_reset(struct xcsi2rxss_state * state)340 static void xcsi2rxss_hard_reset(struct xcsi2rxss_state *state)
341 {
342 	if (!state->rst_gpio)
343 		return;
344 
345 	/* minimum of 40 dphy_clk_200M cycles */
346 	gpiod_set_value_cansleep(state->rst_gpio, 1);
347 	usleep_range(1, 2);
348 	gpiod_set_value_cansleep(state->rst_gpio, 0);
349 }
350 
xcsi2rxss_reset_event_counters(struct xcsi2rxss_state * state)351 static void xcsi2rxss_reset_event_counters(struct xcsi2rxss_state *state)
352 {
353 	unsigned int i;
354 
355 	for (i = 0; i < XCSI_NUM_EVENTS; i++)
356 		state->events[i] = 0;
357 
358 	for (i = 0; i < XCSI_VCX_NUM_EVENTS; i++)
359 		state->vcx_events[i] = 0;
360 }
361 
362 /* Print event counters */
xcsi2rxss_log_counters(struct xcsi2rxss_state * state)363 static void xcsi2rxss_log_counters(struct xcsi2rxss_state *state)
364 {
365 	struct device *dev = state->dev;
366 	unsigned int i;
367 
368 	for (i = 0; i < XCSI_NUM_EVENTS; i++) {
369 		if (state->events[i] > 0) {
370 			dev_info(dev, "%s events: %d\n",
371 				 xcsi2rxss_events[i].name,
372 				 state->events[i]);
373 		}
374 	}
375 
376 	if (state->en_vcx) {
377 		for (i = 0; i < XCSI_VCX_NUM_EVENTS; i++) {
378 			if (state->vcx_events[i] > 0) {
379 				dev_info(dev,
380 					 "VC %d Frame %s err vcx events: %d\n",
381 					 (i / 2) + XCSI_VCX_START,
382 					 i & 1 ? "Sync" : "Level",
383 					 state->vcx_events[i]);
384 			}
385 		}
386 	}
387 }
388 
389 /**
390  * xcsi2rxss_log_status - Logs the status of the CSI-2 Receiver
391  * @sd: Pointer to V4L2 subdevice structure
392  *
393  * This function prints the current status of Xilinx MIPI CSI-2
394  *
395  * Return: 0 on success
396  */
xcsi2rxss_log_status(struct v4l2_subdev * sd)397 static int xcsi2rxss_log_status(struct v4l2_subdev *sd)
398 {
399 	struct xcsi2rxss_state *xcsi2rxss = to_xcsi2rxssstate(sd);
400 	struct device *dev = xcsi2rxss->dev;
401 	u32 reg, data;
402 	unsigned int i, max_vc;
403 
404 	mutex_lock(&xcsi2rxss->lock);
405 
406 	xcsi2rxss_log_counters(xcsi2rxss);
407 
408 	dev_info(dev, "***** Core Status *****\n");
409 	data = xcsi2rxss_read(xcsi2rxss, XCSI_CSR_OFFSET);
410 	dev_info(dev, "Short Packet FIFO Full = %s\n",
411 		 data & XCSI_CSR_SPFIFOFULL ? "true" : "false");
412 	dev_info(dev, "Short Packet FIFO Not Empty = %s\n",
413 		 data & XCSI_CSR_SPFIFONE ? "true" : "false");
414 	dev_info(dev, "Stream line buffer full = %s\n",
415 		 data & XCSI_CSR_SLBF ? "true" : "false");
416 	dev_info(dev, "Soft reset/Core disable in progress = %s\n",
417 		 data & XCSI_CSR_RIPCD ? "true" : "false");
418 
419 	/* Clk & Lane Info  */
420 	dev_info(dev, "******** Clock Lane Info *********\n");
421 	data = xcsi2rxss_read(xcsi2rxss, XCSI_CLKINFR_OFFSET);
422 	dev_info(dev, "Clock Lane in Stop State = %s\n",
423 		 data & XCSI_CLKINFR_STOP ? "true" : "false");
424 
425 	dev_info(dev, "******** Data Lane Info *********\n");
426 	dev_info(dev, "Lane\tSoT Error\tSoT Sync Error\tStop State\n");
427 	reg = XCSI_DLXINFR_OFFSET;
428 	for (i = 0; i < XCSI_MAXDL_COUNT; i++) {
429 		data = xcsi2rxss_read(xcsi2rxss, reg);
430 
431 		dev_info(dev, "%d\t%s\t\t%s\t\t%s\n", i,
432 			 data & XCSI_DLXINFR_SOTERR ? "true" : "false",
433 			 data & XCSI_DLXINFR_SOTSYNCERR ? "true" : "false",
434 			 data & XCSI_DLXINFR_STOP ? "true" : "false");
435 
436 		reg += XCSI_NEXTREG_OFFSET;
437 	}
438 
439 	/* Virtual Channel Image Information */
440 	dev_info(dev, "********** Virtual Channel Info ************\n");
441 	dev_info(dev, "VC\tLine Count\tByte Count\tData Type\n");
442 	if (xcsi2rxss->en_vcx)
443 		max_vc = XCSI_MAX_VCX;
444 	else
445 		max_vc = XCSI_MAX_VC;
446 
447 	reg = XCSI_VCXINF1R_OFFSET;
448 	for (i = 0; i < max_vc; i++) {
449 		u32 line_count, byte_count, data_type;
450 
451 		/* Get line and byte count from VCXINFR1 Register */
452 		data = xcsi2rxss_read(xcsi2rxss, reg);
453 		byte_count = data & XCSI_VCXINF1R_BYTECOUNT;
454 		line_count = data & XCSI_VCXINF1R_LINECOUNT;
455 		line_count >>= XCSI_VCXINF1R_LINECOUNT_SHIFT;
456 
457 		/* Get data type from VCXINFR2 Register */
458 		reg += XCSI_NEXTREG_OFFSET;
459 		data = xcsi2rxss_read(xcsi2rxss, reg);
460 		data_type = data & XCSI_VCXINF2R_DT;
461 
462 		dev_info(dev, "%d\t%d\t\t%d\t\t0x%x\n", i, line_count,
463 			 byte_count, data_type);
464 
465 		/* Move to next pair of VC Info registers */
466 		reg += XCSI_NEXTREG_OFFSET;
467 	}
468 
469 	mutex_unlock(&xcsi2rxss->lock);
470 
471 	return 0;
472 }
473 
xcsi2rxss_get_remote_subdev(struct media_pad * local)474 static struct v4l2_subdev *xcsi2rxss_get_remote_subdev(struct media_pad *local)
475 {
476 	struct media_pad *remote;
477 
478 	remote = media_pad_remote_pad_first(local);
479 	if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
480 		return NULL;
481 
482 	return media_entity_to_v4l2_subdev(remote->entity);
483 }
484 
xcsi2rxss_start_stream(struct xcsi2rxss_state * state)485 static int xcsi2rxss_start_stream(struct xcsi2rxss_state *state)
486 {
487 	int ret = 0;
488 
489 	/* enable core */
490 	xcsi2rxss_set(state, XCSI_CCR_OFFSET, XCSI_CCR_ENABLE);
491 
492 	ret = xcsi2rxss_soft_reset(state);
493 	if (ret) {
494 		state->streaming = false;
495 		return ret;
496 	}
497 
498 	/* enable interrupts */
499 	xcsi2rxss_clr(state, XCSI_GIER_OFFSET, XCSI_GIER_GIE);
500 	xcsi2rxss_write(state, XCSI_IER_OFFSET, XCSI_IER_INTR_MASK);
501 	xcsi2rxss_set(state, XCSI_GIER_OFFSET, XCSI_GIER_GIE);
502 
503 	state->streaming = true;
504 
505 	state->rsubdev =
506 		xcsi2rxss_get_remote_subdev(&state->pads[XVIP_PAD_SINK]);
507 
508 	ret = v4l2_subdev_call(state->rsubdev, video, s_stream, 1);
509 	if (ret) {
510 		/* disable interrupts */
511 		xcsi2rxss_clr(state, XCSI_IER_OFFSET, XCSI_IER_INTR_MASK);
512 		xcsi2rxss_clr(state, XCSI_GIER_OFFSET, XCSI_GIER_GIE);
513 
514 		/* disable core */
515 		xcsi2rxss_clr(state, XCSI_CCR_OFFSET, XCSI_CCR_ENABLE);
516 		state->streaming = false;
517 	}
518 
519 	return ret;
520 }
521 
xcsi2rxss_stop_stream(struct xcsi2rxss_state * state)522 static void xcsi2rxss_stop_stream(struct xcsi2rxss_state *state)
523 {
524 	v4l2_subdev_call(state->rsubdev, video, s_stream, 0);
525 
526 	/* disable interrupts */
527 	xcsi2rxss_clr(state, XCSI_IER_OFFSET, XCSI_IER_INTR_MASK);
528 	xcsi2rxss_clr(state, XCSI_GIER_OFFSET, XCSI_GIER_GIE);
529 
530 	/* disable core */
531 	xcsi2rxss_clr(state, XCSI_CCR_OFFSET, XCSI_CCR_ENABLE);
532 	state->streaming = false;
533 }
534 
535 /**
536  * xcsi2rxss_irq_handler - Interrupt handler for CSI-2
537  * @irq: IRQ number
538  * @data: Pointer to device state
539  *
540  * In the interrupt handler, a list of event counters are updated for
541  * corresponding interrupts. This is useful to get status / debug.
542  *
543  * Return: IRQ_HANDLED after handling interrupts
544  */
xcsi2rxss_irq_handler(int irq,void * data)545 static irqreturn_t xcsi2rxss_irq_handler(int irq, void *data)
546 {
547 	struct xcsi2rxss_state *state = (struct xcsi2rxss_state *)data;
548 	struct device *dev = state->dev;
549 	u32 status;
550 
551 	status = xcsi2rxss_read(state, XCSI_ISR_OFFSET) & XCSI_ISR_ALLINTR_MASK;
552 	xcsi2rxss_write(state, XCSI_ISR_OFFSET, status);
553 
554 	/* Received a short packet */
555 	if (status & XCSI_ISR_SPFIFONE) {
556 		u32 count = 0;
557 
558 		/*
559 		 * Drain generic short packet FIFO by reading max 31
560 		 * (fifo depth) short packets from fifo or till fifo is empty.
561 		 */
562 		for (count = 0; count < XCSI_SPKT_FIFO_DEPTH; ++count) {
563 			u32 spfifostat, spkt;
564 
565 			spkt = xcsi2rxss_read(state, XCSI_SPKTR_OFFSET);
566 			dev_dbg(dev, "Short packet = 0x%08x\n", spkt);
567 			spfifostat = xcsi2rxss_read(state, XCSI_ISR_OFFSET);
568 			spfifostat &= XCSI_ISR_SPFIFONE;
569 			if (!spfifostat)
570 				break;
571 			xcsi2rxss_write(state, XCSI_ISR_OFFSET, spfifostat);
572 		}
573 	}
574 
575 	/* Short packet FIFO overflow */
576 	if (status & XCSI_ISR_SPFIFOF)
577 		dev_dbg_ratelimited(dev, "Short packet FIFO overflowed\n");
578 
579 	/*
580 	 * Stream line buffer full
581 	 * This means there is a backpressure from downstream IP
582 	 */
583 	if (status & XCSI_ISR_SLBF) {
584 		dev_alert_ratelimited(dev, "Stream Line Buffer Full!\n");
585 
586 		/* disable interrupts */
587 		xcsi2rxss_clr(state, XCSI_IER_OFFSET, XCSI_IER_INTR_MASK);
588 		xcsi2rxss_clr(state, XCSI_GIER_OFFSET, XCSI_GIER_GIE);
589 
590 		/* disable core */
591 		xcsi2rxss_clr(state, XCSI_CCR_OFFSET, XCSI_CCR_ENABLE);
592 
593 		/*
594 		 * The IP needs to be hard reset before it can be used now.
595 		 * This will be done in streamoff.
596 		 */
597 
598 		/*
599 		 * TODO: Notify the whole pipeline with v4l2_subdev_notify() to
600 		 * inform userspace.
601 		 */
602 	}
603 
604 	/* Increment event counters */
605 	if (status & XCSI_ISR_ALLINTR_MASK) {
606 		unsigned int i;
607 
608 		for (i = 0; i < XCSI_NUM_EVENTS; i++) {
609 			if (!(status & xcsi2rxss_events[i].mask))
610 				continue;
611 			state->events[i]++;
612 			dev_dbg_ratelimited(dev, "%s: %u\n",
613 					    xcsi2rxss_events[i].name,
614 					    state->events[i]);
615 		}
616 
617 		if (status & XCSI_ISR_VCXFE && state->en_vcx) {
618 			u32 vcxstatus;
619 
620 			vcxstatus = xcsi2rxss_read(state, XCSI_VCXR_OFFSET);
621 			vcxstatus &= XCSI_VCXR_VCERR;
622 			for (i = 0; i < XCSI_VCX_NUM_EVENTS; i++) {
623 				if (!(vcxstatus & BIT(i)))
624 					continue;
625 				state->vcx_events[i]++;
626 			}
627 			xcsi2rxss_write(state, XCSI_VCXR_OFFSET, vcxstatus);
628 		}
629 	}
630 
631 	return IRQ_HANDLED;
632 }
633 
634 /**
635  * xcsi2rxss_s_stream - It is used to start/stop the streaming.
636  * @sd: V4L2 Sub device
637  * @enable: Flag (True / False)
638  *
639  * This function controls the start or stop of streaming for the
640  * Xilinx MIPI CSI-2 Rx Subsystem.
641  *
642  * Return: 0 on success, errors otherwise
643  */
xcsi2rxss_s_stream(struct v4l2_subdev * sd,int enable)644 static int xcsi2rxss_s_stream(struct v4l2_subdev *sd, int enable)
645 {
646 	struct xcsi2rxss_state *xcsi2rxss = to_xcsi2rxssstate(sd);
647 	int ret = 0;
648 
649 	mutex_lock(&xcsi2rxss->lock);
650 
651 	if (enable == xcsi2rxss->streaming)
652 		goto stream_done;
653 
654 	if (enable) {
655 		xcsi2rxss_reset_event_counters(xcsi2rxss);
656 		ret = xcsi2rxss_start_stream(xcsi2rxss);
657 	} else {
658 		xcsi2rxss_stop_stream(xcsi2rxss);
659 		xcsi2rxss_hard_reset(xcsi2rxss);
660 	}
661 
662 stream_done:
663 	mutex_unlock(&xcsi2rxss->lock);
664 	return ret;
665 }
666 
667 static struct v4l2_mbus_framefmt *
__xcsi2rxss_get_pad_format(struct xcsi2rxss_state * xcsi2rxss,struct v4l2_subdev_state * sd_state,unsigned int pad,u32 which)668 __xcsi2rxss_get_pad_format(struct xcsi2rxss_state *xcsi2rxss,
669 			   struct v4l2_subdev_state *sd_state,
670 			   unsigned int pad, u32 which)
671 {
672 	switch (which) {
673 	case V4L2_SUBDEV_FORMAT_TRY:
674 		return v4l2_subdev_get_try_format(&xcsi2rxss->subdev,
675 						  sd_state, pad);
676 	case V4L2_SUBDEV_FORMAT_ACTIVE:
677 		return &xcsi2rxss->format;
678 	default:
679 		return NULL;
680 	}
681 }
682 
683 /**
684  * xcsi2rxss_init_cfg - Initialise the pad format config to default
685  * @sd: Pointer to V4L2 Sub device structure
686  * @sd_state: Pointer to sub device state structure
687  *
688  * This function is used to initialize the pad format with the default
689  * values.
690  *
691  * Return: 0 on success
692  */
xcsi2rxss_init_cfg(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state)693 static int xcsi2rxss_init_cfg(struct v4l2_subdev *sd,
694 			      struct v4l2_subdev_state *sd_state)
695 {
696 	struct xcsi2rxss_state *xcsi2rxss = to_xcsi2rxssstate(sd);
697 	struct v4l2_mbus_framefmt *format;
698 	unsigned int i;
699 
700 	mutex_lock(&xcsi2rxss->lock);
701 	for (i = 0; i < XCSI_MEDIA_PADS; i++) {
702 		format = v4l2_subdev_get_try_format(sd, sd_state, i);
703 		*format = xcsi2rxss->default_format;
704 	}
705 	mutex_unlock(&xcsi2rxss->lock);
706 
707 	return 0;
708 }
709 
710 /**
711  * xcsi2rxss_get_format - Get the pad format
712  * @sd: Pointer to V4L2 Sub device structure
713  * @sd_state: Pointer to sub device state structure
714  * @fmt: Pointer to pad level media bus format
715  *
716  * This function is used to get the pad format information.
717  *
718  * Return: 0 on success
719  */
xcsi2rxss_get_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)720 static int xcsi2rxss_get_format(struct v4l2_subdev *sd,
721 				struct v4l2_subdev_state *sd_state,
722 				struct v4l2_subdev_format *fmt)
723 {
724 	struct xcsi2rxss_state *xcsi2rxss = to_xcsi2rxssstate(sd);
725 
726 	mutex_lock(&xcsi2rxss->lock);
727 	fmt->format = *__xcsi2rxss_get_pad_format(xcsi2rxss, sd_state,
728 						  fmt->pad,
729 						  fmt->which);
730 	mutex_unlock(&xcsi2rxss->lock);
731 
732 	return 0;
733 }
734 
735 /**
736  * xcsi2rxss_set_format - This is used to set the pad format
737  * @sd: Pointer to V4L2 Sub device structure
738  * @sd_state: Pointer to sub device state structure
739  * @fmt: Pointer to pad level media bus format
740  *
741  * This function is used to set the pad format. Since the pad format is fixed
742  * in hardware, it can't be modified on run time. So when a format set is
743  * requested by application, all parameters except the format type is saved
744  * for the pad and the original pad format is sent back to the application.
745  *
746  * Return: 0 on success
747  */
xcsi2rxss_set_format(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_format * fmt)748 static int xcsi2rxss_set_format(struct v4l2_subdev *sd,
749 				struct v4l2_subdev_state *sd_state,
750 				struct v4l2_subdev_format *fmt)
751 {
752 	struct xcsi2rxss_state *xcsi2rxss = to_xcsi2rxssstate(sd);
753 	struct v4l2_mbus_framefmt *__format;
754 	u32 dt;
755 
756 	mutex_lock(&xcsi2rxss->lock);
757 
758 	/*
759 	 * Only the format->code parameter matters for CSI as the
760 	 * CSI format cannot be changed at runtime.
761 	 * Ensure that format to set is copied to over to CSI pad format
762 	 */
763 	__format = __xcsi2rxss_get_pad_format(xcsi2rxss, sd_state,
764 					      fmt->pad, fmt->which);
765 
766 	/* only sink pad format can be updated */
767 	if (fmt->pad == XVIP_PAD_SOURCE) {
768 		fmt->format = *__format;
769 		mutex_unlock(&xcsi2rxss->lock);
770 		return 0;
771 	}
772 
773 	/*
774 	 * RAW8 is supported in all datatypes. So if requested media bus format
775 	 * is of RAW8 type, then allow to be set. In case core is configured to
776 	 * other RAW, YUV422 8/10 or RGB888, set appropriate media bus format.
777 	 */
778 	dt = xcsi2rxss_get_dt(fmt->format.code);
779 	if (dt != xcsi2rxss->datatype && dt != MIPI_CSI2_DT_RAW8) {
780 		dev_dbg(xcsi2rxss->dev, "Unsupported media bus format");
781 		/* set the default format for the data type */
782 		fmt->format.code = xcsi2rxss_get_nth_mbus(xcsi2rxss->datatype,
783 							  0);
784 	}
785 
786 	*__format = fmt->format;
787 	mutex_unlock(&xcsi2rxss->lock);
788 
789 	return 0;
790 }
791 
792 /*
793  * xcsi2rxss_enum_mbus_code - Handle pixel format enumeration
794  * @sd: pointer to v4l2 subdev structure
795  * @cfg: V4L2 subdev pad configuration
796  * @code: pointer to v4l2_subdev_mbus_code_enum structure
797  *
798  * Return: -EINVAL or zero on success
799  */
xcsi2rxss_enum_mbus_code(struct v4l2_subdev * sd,struct v4l2_subdev_state * sd_state,struct v4l2_subdev_mbus_code_enum * code)800 static int xcsi2rxss_enum_mbus_code(struct v4l2_subdev *sd,
801 				    struct v4l2_subdev_state *sd_state,
802 				    struct v4l2_subdev_mbus_code_enum *code)
803 {
804 	struct xcsi2rxss_state *state = to_xcsi2rxssstate(sd);
805 	u32 dt, n;
806 	int ret = 0;
807 
808 	/* RAW8 dt packets are available in all DT configurations */
809 	if (code->index < 4) {
810 		n = code->index;
811 		dt = MIPI_CSI2_DT_RAW8;
812 	} else if (state->datatype != MIPI_CSI2_DT_RAW8) {
813 		n = code->index - 4;
814 		dt = state->datatype;
815 	} else {
816 		return -EINVAL;
817 	}
818 
819 	code->code = xcsi2rxss_get_nth_mbus(dt, n);
820 	if (!code->code)
821 		ret = -EINVAL;
822 
823 	return ret;
824 }
825 
826 /* -----------------------------------------------------------------------------
827  * Media Operations
828  */
829 
830 static const struct media_entity_operations xcsi2rxss_media_ops = {
831 	.link_validate = v4l2_subdev_link_validate
832 };
833 
834 static const struct v4l2_subdev_core_ops xcsi2rxss_core_ops = {
835 	.log_status = xcsi2rxss_log_status,
836 };
837 
838 static const struct v4l2_subdev_video_ops xcsi2rxss_video_ops = {
839 	.s_stream = xcsi2rxss_s_stream
840 };
841 
842 static const struct v4l2_subdev_pad_ops xcsi2rxss_pad_ops = {
843 	.init_cfg = xcsi2rxss_init_cfg,
844 	.get_fmt = xcsi2rxss_get_format,
845 	.set_fmt = xcsi2rxss_set_format,
846 	.enum_mbus_code = xcsi2rxss_enum_mbus_code,
847 	.link_validate = v4l2_subdev_link_validate_default,
848 };
849 
850 static const struct v4l2_subdev_ops xcsi2rxss_ops = {
851 	.core = &xcsi2rxss_core_ops,
852 	.video = &xcsi2rxss_video_ops,
853 	.pad = &xcsi2rxss_pad_ops
854 };
855 
xcsi2rxss_parse_of(struct xcsi2rxss_state * xcsi2rxss)856 static int xcsi2rxss_parse_of(struct xcsi2rxss_state *xcsi2rxss)
857 {
858 	struct device *dev = xcsi2rxss->dev;
859 	struct device_node *node = dev->of_node;
860 
861 	struct fwnode_handle *ep;
862 	struct v4l2_fwnode_endpoint vep = {
863 		.bus_type = V4L2_MBUS_CSI2_DPHY
864 	};
865 	bool en_csi_v20, vfb;
866 	int ret;
867 
868 	en_csi_v20 = of_property_read_bool(node, "xlnx,en-csi-v2-0");
869 	if (en_csi_v20)
870 		xcsi2rxss->en_vcx = of_property_read_bool(node, "xlnx,en-vcx");
871 
872 	xcsi2rxss->enable_active_lanes =
873 		of_property_read_bool(node, "xlnx,en-active-lanes");
874 
875 	ret = of_property_read_u32(node, "xlnx,csi-pxl-format",
876 				   &xcsi2rxss->datatype);
877 	if (ret < 0) {
878 		dev_err(dev, "missing xlnx,csi-pxl-format property\n");
879 		return ret;
880 	}
881 
882 	switch (xcsi2rxss->datatype) {
883 	case MIPI_CSI2_DT_YUV422_8B:
884 	case MIPI_CSI2_DT_RGB444:
885 	case MIPI_CSI2_DT_RGB555:
886 	case MIPI_CSI2_DT_RGB565:
887 	case MIPI_CSI2_DT_RGB666:
888 	case MIPI_CSI2_DT_RGB888:
889 	case MIPI_CSI2_DT_RAW6:
890 	case MIPI_CSI2_DT_RAW7:
891 	case MIPI_CSI2_DT_RAW8:
892 	case MIPI_CSI2_DT_RAW10:
893 	case MIPI_CSI2_DT_RAW12:
894 	case MIPI_CSI2_DT_RAW14:
895 		break;
896 	case MIPI_CSI2_DT_YUV422_10B:
897 	case MIPI_CSI2_DT_RAW16:
898 	case MIPI_CSI2_DT_RAW20:
899 		if (!en_csi_v20) {
900 			ret = -EINVAL;
901 			dev_dbg(dev, "enable csi v2 for this pixel format");
902 		}
903 		break;
904 	default:
905 		ret = -EINVAL;
906 	}
907 	if (ret < 0) {
908 		dev_err(dev, "invalid csi-pxl-format property!\n");
909 		return ret;
910 	}
911 
912 	vfb = of_property_read_bool(node, "xlnx,vfb");
913 	if (!vfb) {
914 		dev_err(dev, "operation without VFB is not supported\n");
915 		return -EINVAL;
916 	}
917 
918 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
919 					     XVIP_PAD_SINK, 0,
920 					     FWNODE_GRAPH_ENDPOINT_NEXT);
921 	if (!ep) {
922 		dev_err(dev, "no sink port found");
923 		return -EINVAL;
924 	}
925 
926 	ret = v4l2_fwnode_endpoint_parse(ep, &vep);
927 	fwnode_handle_put(ep);
928 	if (ret) {
929 		dev_err(dev, "error parsing sink port");
930 		return ret;
931 	}
932 
933 	dev_dbg(dev, "mipi number lanes = %d\n",
934 		vep.bus.mipi_csi2.num_data_lanes);
935 
936 	xcsi2rxss->max_num_lanes = vep.bus.mipi_csi2.num_data_lanes;
937 
938 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
939 					     XVIP_PAD_SOURCE, 0,
940 					     FWNODE_GRAPH_ENDPOINT_NEXT);
941 	if (!ep) {
942 		dev_err(dev, "no source port found");
943 		return -EINVAL;
944 	}
945 
946 	fwnode_handle_put(ep);
947 
948 	dev_dbg(dev, "vcx %s, %u data lanes (%s), data type 0x%02x\n",
949 		xcsi2rxss->en_vcx ? "enabled" : "disabled",
950 		xcsi2rxss->max_num_lanes,
951 		xcsi2rxss->enable_active_lanes ? "dynamic" : "static",
952 		xcsi2rxss->datatype);
953 
954 	return 0;
955 }
956 
xcsi2rxss_probe(struct platform_device * pdev)957 static int xcsi2rxss_probe(struct platform_device *pdev)
958 {
959 	struct v4l2_subdev *subdev;
960 	struct xcsi2rxss_state *xcsi2rxss;
961 	int num_clks = ARRAY_SIZE(xcsi2rxss_clks);
962 	struct device *dev = &pdev->dev;
963 	int irq, ret;
964 
965 	xcsi2rxss = devm_kzalloc(dev, sizeof(*xcsi2rxss), GFP_KERNEL);
966 	if (!xcsi2rxss)
967 		return -ENOMEM;
968 
969 	xcsi2rxss->dev = dev;
970 
971 	xcsi2rxss->clks = devm_kmemdup(dev, xcsi2rxss_clks,
972 				       sizeof(xcsi2rxss_clks), GFP_KERNEL);
973 	if (!xcsi2rxss->clks)
974 		return -ENOMEM;
975 
976 	/* Reset GPIO */
977 	xcsi2rxss->rst_gpio = devm_gpiod_get_optional(dev, "video-reset",
978 						      GPIOD_OUT_HIGH);
979 	if (IS_ERR(xcsi2rxss->rst_gpio))
980 		return dev_err_probe(dev, PTR_ERR(xcsi2rxss->rst_gpio),
981 				     "Video Reset GPIO not setup in DT\n");
982 
983 	ret = xcsi2rxss_parse_of(xcsi2rxss);
984 	if (ret < 0)
985 		return ret;
986 
987 	xcsi2rxss->iomem = devm_platform_ioremap_resource(pdev, 0);
988 	if (IS_ERR(xcsi2rxss->iomem))
989 		return PTR_ERR(xcsi2rxss->iomem);
990 
991 	irq = platform_get_irq(pdev, 0);
992 	if (irq < 0)
993 		return irq;
994 
995 	ret = devm_request_threaded_irq(dev, irq, NULL,
996 					xcsi2rxss_irq_handler, IRQF_ONESHOT,
997 					dev_name(dev), xcsi2rxss);
998 	if (ret) {
999 		dev_err(dev, "Err = %d Interrupt handler reg failed!\n", ret);
1000 		return ret;
1001 	}
1002 
1003 	ret = clk_bulk_get(dev, num_clks, xcsi2rxss->clks);
1004 	if (ret)
1005 		return ret;
1006 
1007 	/* TODO: Enable/disable clocks at stream on/off time. */
1008 	ret = clk_bulk_prepare_enable(num_clks, xcsi2rxss->clks);
1009 	if (ret)
1010 		goto err_clk_put;
1011 
1012 	mutex_init(&xcsi2rxss->lock);
1013 
1014 	xcsi2rxss_hard_reset(xcsi2rxss);
1015 	xcsi2rxss_soft_reset(xcsi2rxss);
1016 
1017 	/* Initialize V4L2 subdevice and media entity */
1018 	xcsi2rxss->pads[XVIP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1019 	xcsi2rxss->pads[XVIP_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1020 
1021 	/* Initialize the default format */
1022 	xcsi2rxss->default_format.code =
1023 		xcsi2rxss_get_nth_mbus(xcsi2rxss->datatype, 0);
1024 	xcsi2rxss->default_format.field = V4L2_FIELD_NONE;
1025 	xcsi2rxss->default_format.colorspace = V4L2_COLORSPACE_SRGB;
1026 	xcsi2rxss->default_format.width = XCSI_DEFAULT_WIDTH;
1027 	xcsi2rxss->default_format.height = XCSI_DEFAULT_HEIGHT;
1028 	xcsi2rxss->format = xcsi2rxss->default_format;
1029 
1030 	/* Initialize V4L2 subdevice and media entity */
1031 	subdev = &xcsi2rxss->subdev;
1032 	v4l2_subdev_init(subdev, &xcsi2rxss_ops);
1033 	subdev->dev = dev;
1034 	strscpy(subdev->name, dev_name(dev), sizeof(subdev->name));
1035 	subdev->flags |= V4L2_SUBDEV_FL_HAS_EVENTS | V4L2_SUBDEV_FL_HAS_DEVNODE;
1036 	subdev->entity.ops = &xcsi2rxss_media_ops;
1037 	v4l2_set_subdevdata(subdev, xcsi2rxss);
1038 
1039 	ret = media_entity_pads_init(&subdev->entity, XCSI_MEDIA_PADS,
1040 				     xcsi2rxss->pads);
1041 	if (ret < 0)
1042 		goto error;
1043 
1044 	platform_set_drvdata(pdev, xcsi2rxss);
1045 
1046 	ret = v4l2_async_register_subdev(subdev);
1047 	if (ret < 0) {
1048 		dev_err(dev, "failed to register subdev\n");
1049 		goto error;
1050 	}
1051 
1052 	return 0;
1053 error:
1054 	media_entity_cleanup(&subdev->entity);
1055 	mutex_destroy(&xcsi2rxss->lock);
1056 	clk_bulk_disable_unprepare(num_clks, xcsi2rxss->clks);
1057 err_clk_put:
1058 	clk_bulk_put(num_clks, xcsi2rxss->clks);
1059 	return ret;
1060 }
1061 
xcsi2rxss_remove(struct platform_device * pdev)1062 static void xcsi2rxss_remove(struct platform_device *pdev)
1063 {
1064 	struct xcsi2rxss_state *xcsi2rxss = platform_get_drvdata(pdev);
1065 	struct v4l2_subdev *subdev = &xcsi2rxss->subdev;
1066 	int num_clks = ARRAY_SIZE(xcsi2rxss_clks);
1067 
1068 	v4l2_async_unregister_subdev(subdev);
1069 	media_entity_cleanup(&subdev->entity);
1070 	mutex_destroy(&xcsi2rxss->lock);
1071 	clk_bulk_disable_unprepare(num_clks, xcsi2rxss->clks);
1072 	clk_bulk_put(num_clks, xcsi2rxss->clks);
1073 }
1074 
1075 static const struct of_device_id xcsi2rxss_of_id_table[] = {
1076 	{ .compatible = "xlnx,mipi-csi2-rx-subsystem-5.0", },
1077 	{ }
1078 };
1079 MODULE_DEVICE_TABLE(of, xcsi2rxss_of_id_table);
1080 
1081 static struct platform_driver xcsi2rxss_driver = {
1082 	.driver = {
1083 		.name		= "xilinx-csi2rxss",
1084 		.of_match_table	= xcsi2rxss_of_id_table,
1085 	},
1086 	.probe			= xcsi2rxss_probe,
1087 	.remove_new		= xcsi2rxss_remove,
1088 };
1089 
1090 module_platform_driver(xcsi2rxss_driver);
1091 
1092 MODULE_AUTHOR("Vishal Sagar <vsagar@xilinx.com>");
1093 MODULE_DESCRIPTION("Xilinx MIPI CSI-2 Rx Subsystem Driver");
1094 MODULE_LICENSE("GPL v2");
1095