xref: /openbmc/linux/drivers/media/i2c/tc358746.c (revision 6b43538f)
1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  * TC358746 - Parallel <-> CSI-2 Bridge
4  *
5  * Copyright 2022 Marco Felsch <kernel@pengutronix.de>
6  *
7  * Notes:
8  *  - Currently only 'Parallel-in -> CSI-out' mode is supported!
9  */
10 
11 #include <linux/bitfield.h>
12 #include <linux/clk.h>
13 #include <linux/clk-provider.h>
14 #include <linux/delay.h>
15 #include <linux/i2c.h>
16 #include <linux/interrupt.h>
17 #include <linux/kernel.h>
18 #include <linux/module.h>
19 #include <linux/phy/phy-mipi-dphy.h>
20 #include <linux/property.h>
21 #include <linux/pm_runtime.h>
22 #include <linux/regmap.h>
23 #include <linux/units.h>
24 #include <media/v4l2-ctrls.h>
25 #include <media/v4l2-device.h>
26 #include <media/v4l2-fwnode.h>
27 #include <media/v4l2-mc.h>
28 
29 /* 16-bit registers */
30 #define CHIPID_REG			0x0000
31 #define		CHIPID			GENMASK(15, 8)
32 
33 #define SYSCTL_REG			0x0002
34 #define		SRESET			BIT(0)
35 
36 #define CONFCTL_REG			0x0004
37 #define		PDATAF_MASK		GENMASK(9, 8)
38 #define		PDATAF_MODE0		0
39 #define		PDATAF_MODE1		1
40 #define		PDATAF_MODE2		2
41 #define		PDATAF(val)		FIELD_PREP(PDATAF_MASK, (val))
42 #define		PPEN			BIT(6)
43 #define		DATALANE_MASK		GENMASK(1, 0)
44 
45 #define FIFOCTL_REG			0x0006
46 #define DATAFMT_REG			0x0008
47 #define		PDFMT(val)		FIELD_PREP(GENMASK(7, 4), (val))
48 
49 #define MCLKCTL_REG			0x000c
50 #define		MCLK_HIGH_MASK		GENMASK(15, 8)
51 #define		MCLK_LOW_MASK		GENMASK(7, 0)
52 #define		MCLK_HIGH(val)		FIELD_PREP(MCLK_HIGH_MASK, (val))
53 #define		MCLK_LOW(val)		FIELD_PREP(MCLK_LOW_MASK, (val))
54 
55 #define PLLCTL0_REG			0x0016
56 #define		PLL_PRD_MASK		GENMASK(15, 12)
57 #define		PLL_PRD(val)		FIELD_PREP(PLL_PRD_MASK, (val))
58 #define		PLL_FBD_MASK		GENMASK(8, 0)
59 #define		PLL_FBD(val)		FIELD_PREP(PLL_FBD_MASK, (val))
60 
61 #define PLLCTL1_REG			0x0018
62 #define		PLL_FRS_MASK		GENMASK(11, 10)
63 #define		PLL_FRS(val)		FIELD_PREP(PLL_FRS_MASK, (val))
64 #define		CKEN			BIT(4)
65 #define		RESETB			BIT(1)
66 #define		PLL_EN			BIT(0)
67 
68 #define CLKCTL_REG			0x0020
69 #define		MCLKDIV_MASK		GENMASK(3, 2)
70 #define		MCLKDIV(val)		FIELD_PREP(MCLKDIV_MASK, (val))
71 #define		MCLKDIV_8		0
72 #define		MCLKDIV_4		1
73 #define		MCLKDIV_2		2
74 
75 #define WORDCNT_REG			0x0022
76 #define PP_MISC_REG			0x0032
77 #define		FRMSTOP			BIT(15)
78 #define		RSTPTR			BIT(14)
79 
80 /* 32-bit registers */
81 #define CLW_DPHYCONTTX_REG		0x0100
82 #define CLW_CNTRL_REG			0x0140
83 #define D0W_CNTRL_REG			0x0144
84 #define		LANEDISABLE		BIT(0)
85 
86 #define STARTCNTRL_REG			0x0204
87 #define		START			BIT(0)
88 
89 #define LINEINITCNT_REG			0x0210
90 #define LPTXTIMECNT_REG			0x0214
91 #define TCLK_HEADERCNT_REG		0x0218
92 #define		TCLK_ZEROCNT(val)	FIELD_PREP(GENMASK(15, 8), (val))
93 #define		TCLK_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
94 
95 #define TCLK_TRAILCNT_REG		0x021C
96 #define THS_HEADERCNT_REG		0x0220
97 #define		THS_ZEROCNT(val)	FIELD_PREP(GENMASK(14, 8), (val))
98 #define		THS_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
99 
100 #define TWAKEUP_REG			0x0224
101 #define TCLK_POSTCNT_REG		0x0228
102 #define THS_TRAILCNT_REG		0x022C
103 #define HSTXVREGEN_REG			0x0234
104 #define TXOPTIONCNTRL_REG		0x0238
105 #define CSI_CONTROL_REG			0x040C
106 #define		CSI_MODE		BIT(15)
107 #define		TXHSMD			BIT(7)
108 #define		NOL(val)		FIELD_PREP(GENMASK(2, 1), (val))
109 
110 #define CSI_CONFW_REG			0x0500
111 #define		MODE(val)		FIELD_PREP(GENMASK(31, 29), (val))
112 #define		MODE_SET		0x5
113 #define		ADDRESS(val)		FIELD_PREP(GENMASK(28, 24), (val))
114 #define		CSI_CONTROL_ADDRESS	0x3
115 #define		DATA(val)		FIELD_PREP(GENMASK(15, 0), (val))
116 
117 #define CSI_START_REG			0x0518
118 #define		STRT			BIT(0)
119 
120 static const struct v4l2_mbus_framefmt tc358746_def_fmt = {
121 	.width		= 640,
122 	.height		= 480,
123 	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
124 	.field		= V4L2_FIELD_NONE,
125 	.colorspace	= V4L2_COLORSPACE_DEFAULT,
126 	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
127 	.quantization	= V4L2_QUANTIZATION_DEFAULT,
128 	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
129 };
130 
131 static const char * const tc358746_supplies[] = {
132 	"vddc", "vddio", "vddmipi"
133 };
134 
135 enum {
136 	TC358746_SINK,
137 	TC358746_SOURCE,
138 	TC358746_NR_PADS
139 };
140 
141 struct tc358746 {
142 	struct v4l2_subdev		sd;
143 	struct media_pad		pads[TC358746_NR_PADS];
144 	struct v4l2_async_notifier	notifier;
145 	struct v4l2_fwnode_endpoint	csi_vep;
146 
147 	struct v4l2_ctrl_handler	ctrl_hdl;
148 
149 	struct regmap			*regmap;
150 	struct clk			*refclk;
151 	struct gpio_desc		*reset_gpio;
152 	struct regulator_bulk_data	supplies[ARRAY_SIZE(tc358746_supplies)];
153 
154 	struct clk_hw			mclk_hw;
155 	unsigned long			mclk_rate;
156 	u8				mclk_prediv;
157 	u16				mclk_postdiv;
158 
159 	unsigned long			pll_rate;
160 	u8				pll_post_div;
161 	u16				pll_pre_div;
162 	u16				pll_mul;
163 
164 #define TC358746_VB_MAX_SIZE		(511 * 32)
165 #define TC358746_VB_DEFAULT_SIZE	  (1 * 32)
166 	unsigned int			vb_size; /* Video buffer size in bits */
167 
168 	struct phy_configure_opts_mipi_dphy dphy_cfg;
169 };
170 
171 static inline struct tc358746 *to_tc358746(struct v4l2_subdev *sd)
172 {
173 	return container_of(sd, struct tc358746, sd);
174 }
175 
176 static inline struct tc358746 *clk_hw_to_tc358746(struct clk_hw *hw)
177 {
178 	return container_of(hw, struct tc358746, mclk_hw);
179 }
180 
181 struct tc358746_format {
182 	u32		code;
183 	bool		csi_format;
184 	unsigned char	bus_width;
185 	unsigned char	bpp;
186 	/* Register values */
187 	u8		pdformat; /* Peripheral Data Format */
188 	u8		pdataf;   /* Parallel Data Format Option */
189 };
190 
191 enum {
192 	PDFORMAT_RAW8 = 0,
193 	PDFORMAT_RAW10,
194 	PDFORMAT_RAW12,
195 	PDFORMAT_RGB888,
196 	PDFORMAT_RGB666,
197 	PDFORMAT_RGB565,
198 	PDFORMAT_YUV422_8BIT,
199 	/* RESERVED = 7 */
200 	PDFORMAT_RAW14 = 8,
201 	PDFORMAT_YUV422_10BIT,
202 	PDFORMAT_YUV444,
203 };
204 
205 /* Check tc358746_src_mbus_code() if you add new formats */
206 static const struct tc358746_format tc358746_formats[] = {
207 	{
208 		.code = MEDIA_BUS_FMT_UYVY8_2X8,
209 		.bus_width = 8,
210 		.bpp = 16,
211 		.pdformat = PDFORMAT_YUV422_8BIT,
212 		.pdataf = PDATAF_MODE0,
213 	}, {
214 		.code = MEDIA_BUS_FMT_UYVY8_1X16,
215 		.csi_format = true,
216 		.bus_width = 16,
217 		.bpp = 16,
218 		.pdformat = PDFORMAT_YUV422_8BIT,
219 		.pdataf = PDATAF_MODE1,
220 	}, {
221 		.code = MEDIA_BUS_FMT_YUYV8_1X16,
222 		.csi_format = true,
223 		.bus_width = 16,
224 		.bpp = 16,
225 		.pdformat = PDFORMAT_YUV422_8BIT,
226 		.pdataf = PDATAF_MODE2,
227 	}, {
228 		.code = MEDIA_BUS_FMT_UYVY10_2X10,
229 		.bus_width = 10,
230 		.bpp = 20,
231 		.pdformat = PDFORMAT_YUV422_10BIT,
232 		.pdataf = PDATAF_MODE0, /* don't care */
233 	}
234 };
235 
236 /* Get n-th format for pad */
237 static const struct tc358746_format *
238 tc358746_get_format_by_idx(unsigned int pad, unsigned int index)
239 {
240 	unsigned int idx = 0;
241 	unsigned int i;
242 
243 	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
244 		const struct tc358746_format *fmt = &tc358746_formats[i];
245 
246 		if ((pad == TC358746_SOURCE && fmt->csi_format) ||
247 		    (pad == TC358746_SINK)) {
248 			if (idx == index)
249 				return fmt;
250 			idx++;
251 		}
252 	}
253 
254 	return ERR_PTR(-EINVAL);
255 }
256 
257 static const struct tc358746_format *
258 tc358746_get_format_by_code(unsigned int pad, u32 code)
259 {
260 	unsigned int i;
261 
262 	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
263 		const struct tc358746_format *fmt = &tc358746_formats[i];
264 
265 		if (pad == TC358746_SINK && fmt->code == code)
266 			return fmt;
267 
268 		if (pad == TC358746_SOURCE && !fmt->csi_format)
269 			continue;
270 
271 		if (fmt->code == code)
272 			return fmt;
273 	}
274 
275 	return ERR_PTR(-EINVAL);
276 }
277 
278 static u32 tc358746_src_mbus_code(u32 code)
279 {
280 	switch (code) {
281 	case MEDIA_BUS_FMT_UYVY8_2X8:
282 		return MEDIA_BUS_FMT_UYVY8_1X16;
283 	case MEDIA_BUS_FMT_UYVY10_2X10:
284 		return MEDIA_BUS_FMT_UYVY10_1X20;
285 	default:
286 		return code;
287 	}
288 }
289 
290 static bool tc358746_valid_reg(struct device *dev, unsigned int reg)
291 {
292 	switch (reg) {
293 	case CHIPID_REG ... CSI_START_REG:
294 		return true;
295 	default:
296 		return false;
297 	}
298 }
299 
300 static const struct regmap_config tc358746_regmap_config = {
301 	.name = "tc358746",
302 	.reg_bits = 16,
303 	.val_bits = 16,
304 	.max_register = CSI_START_REG,
305 	.writeable_reg = tc358746_valid_reg,
306 	.readable_reg = tc358746_valid_reg,
307 	.reg_format_endian = REGMAP_ENDIAN_BIG,
308 	.val_format_endian = REGMAP_ENDIAN_BIG,
309 };
310 
311 static int tc358746_write(struct tc358746 *tc358746, u32 reg, u32 val)
312 {
313 	size_t count;
314 	int err;
315 
316 	/* 32-bit registers starting from CLW_DPHYCONTTX */
317 	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
318 
319 	err = regmap_bulk_write(tc358746->regmap, reg, &val, count);
320 	if (err)
321 		dev_err(tc358746->sd.dev,
322 			"Failed to write reg:0x%04x err:%d\n", reg, err);
323 
324 	return err;
325 }
326 
327 static int tc358746_read(struct tc358746 *tc358746, u32 reg, u32 *val)
328 {
329 	size_t count;
330 	int err;
331 
332 	/* 32-bit registers starting from CLW_DPHYCONTTX */
333 	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
334 	*val = 0;
335 
336 	err = regmap_bulk_read(tc358746->regmap, reg, val, count);
337 	if (err)
338 		dev_err(tc358746->sd.dev,
339 			"Failed to read reg:0x%04x err:%d\n", reg, err);
340 
341 	return err;
342 }
343 
344 static int
345 tc358746_update_bits(struct tc358746 *tc358746, u32 reg, u32 mask, u32 val)
346 {
347 	u32 tmp, orig;
348 	int err;
349 
350 	err = tc358746_read(tc358746, reg, &orig);
351 	if (err)
352 		return err;
353 
354 	tmp = orig & ~mask;
355 	tmp |= val & mask;
356 
357 	return tc358746_write(tc358746, reg, tmp);
358 }
359 
360 static int tc358746_set_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
361 {
362 	return tc358746_update_bits(tc358746, reg, bits, bits);
363 }
364 
365 static int tc358746_clear_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
366 {
367 	return tc358746_update_bits(tc358746, reg, bits, 0);
368 }
369 
370 static int tc358746_sw_reset(struct tc358746 *tc358746)
371 {
372 	int err;
373 
374 	err = tc358746_set_bits(tc358746, SYSCTL_REG, SRESET);
375 	if (err)
376 		return err;
377 
378 	fsleep(10);
379 
380 	return tc358746_clear_bits(tc358746, SYSCTL_REG, SRESET);
381 }
382 
383 static int
384 tc358746_apply_pll_config(struct tc358746 *tc358746)
385 {
386 	u8 post = tc358746->pll_post_div;
387 	u16 pre = tc358746->pll_pre_div;
388 	u16 mul = tc358746->pll_mul;
389 	u32 val, mask;
390 	int err;
391 
392 	err = tc358746_read(tc358746, PLLCTL1_REG, &val);
393 	if (err)
394 		return err;
395 
396 	/* Don't touch the PLL if running */
397 	if (FIELD_GET(PLL_EN, val) == 1)
398 		return 0;
399 
400 	/* Pre-div and Multiplicator have a internal +1 logic */
401 	val = PLL_PRD(pre - 1) | PLL_FBD(mul - 1);
402 	mask = PLL_PRD_MASK | PLL_FBD_MASK;
403 	err = tc358746_update_bits(tc358746, PLLCTL0_REG, mask, val);
404 	if (err)
405 		return err;
406 
407 	val = PLL_FRS(ilog2(post)) | RESETB | PLL_EN;
408 	mask = PLL_FRS_MASK | RESETB | PLL_EN;
409 	tc358746_update_bits(tc358746, PLLCTL1_REG, mask, val);
410 	if (err)
411 		return err;
412 
413 	fsleep(1000);
414 
415 	return tc358746_set_bits(tc358746, PLLCTL1_REG, CKEN);
416 }
417 
418 static int tc358746_apply_misc_config(struct tc358746 *tc358746)
419 {
420 	const struct v4l2_mbus_framefmt *mbusfmt;
421 	struct v4l2_subdev *sd = &tc358746->sd;
422 	struct v4l2_subdev_state *sink_state;
423 	const struct tc358746_format *fmt;
424 	struct device *dev = sd->dev;
425 	u32 val;
426 	int err;
427 
428 	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
429 
430 	mbusfmt = v4l2_subdev_get_pad_format(sd, sink_state, TC358746_SINK);
431 	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
432 
433 	/* Self defined CSI user data type id's are not supported yet */
434 	val = PDFMT(fmt->pdformat);
435 	dev_dbg(dev, "DATAFMT: 0x%x\n", val);
436 	err = tc358746_write(tc358746, DATAFMT_REG, val);
437 	if (err)
438 		goto out;
439 
440 	val = PDATAF(fmt->pdataf);
441 	dev_dbg(dev, "CONFCTL[PDATAF]: 0x%x\n", fmt->pdataf);
442 	err = tc358746_update_bits(tc358746, CONFCTL_REG, PDATAF_MASK, val);
443 	if (err)
444 		goto out;
445 
446 	val = tc358746->vb_size / 32;
447 	dev_dbg(dev, "FIFOCTL: %u (0x%x)\n", val, val);
448 	err = tc358746_write(tc358746, FIFOCTL_REG, val);
449 	if (err)
450 		goto out;
451 
452 	/* Total number of bytes for each line/width */
453 	val = mbusfmt->width * fmt->bpp / 8;
454 	dev_dbg(dev, "WORDCNT: %u (0x%x)\n", val, val);
455 	err = tc358746_write(tc358746, WORDCNT_REG, val);
456 
457 out:
458 	v4l2_subdev_unlock_state(sink_state);
459 
460 	return err;
461 }
462 
463 /* Use MHz as base so the div needs no u64 */
464 static u32 tc358746_cfg_to_cnt(unsigned int cfg_val,
465 			       unsigned int clk_mhz,
466 			       unsigned int time_base)
467 {
468 	return DIV_ROUND_UP(cfg_val * clk_mhz, time_base);
469 }
470 
471 static u32 tc358746_ps_to_cnt(unsigned int cfg_val,
472 			      unsigned int clk_mhz)
473 {
474 	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, USEC_PER_SEC);
475 }
476 
477 static u32 tc358746_us_to_cnt(unsigned int cfg_val,
478 			      unsigned int clk_mhz)
479 {
480 	return tc358746_cfg_to_cnt(cfg_val, clk_mhz, 1);
481 }
482 
483 static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
484 {
485 	struct phy_configure_opts_mipi_dphy *cfg = &tc358746->dphy_cfg;
486 	bool non_cont_clk = !!(tc358746->csi_vep.bus.mipi_csi2.flags &
487 			       V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK);
488 	struct device *dev = tc358746->sd.dev;
489 	unsigned long hs_byte_clk, hf_clk;
490 	u32 val, val2, lptxcnt;
491 	int err;
492 
493 	/* The hs_byte_clk is also called SYSCLK in the excel sheet */
494 	hs_byte_clk = cfg->hs_clk_rate / 8;
495 	hs_byte_clk /= HZ_PER_MHZ;
496 	hf_clk = hs_byte_clk / 2;
497 
498 	val = tc358746_us_to_cnt(cfg->init, hf_clk) - 1;
499 	dev_dbg(dev, "LINEINITCNT: %u (0x%x)\n", val, val);
500 	err = tc358746_write(tc358746, LINEINITCNT_REG, val);
501 	if (err)
502 		return err;
503 
504 	val = tc358746_ps_to_cnt(cfg->lpx, hs_byte_clk) - 1;
505 	lptxcnt = val;
506 	dev_dbg(dev, "LPTXTIMECNT: %u (0x%x)\n", val, val);
507 	err = tc358746_write(tc358746, LPTXTIMECNT_REG, val);
508 	if (err)
509 		return err;
510 
511 	val = tc358746_ps_to_cnt(cfg->clk_prepare, hs_byte_clk) - 1;
512 	val2 = tc358746_ps_to_cnt(cfg->clk_zero, hs_byte_clk) - 1;
513 	dev_dbg(dev, "TCLK_PREPARECNT: %u (0x%x)\n", val, val);
514 	dev_dbg(dev, "TCLK_ZEROCNT: %u (0x%x)\n", val2, val2);
515 	dev_dbg(dev, "TCLK_HEADERCNT: 0x%x\n",
516 		(u32)(TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2)));
517 	err = tc358746_write(tc358746, TCLK_HEADERCNT_REG,
518 			     TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2));
519 	if (err)
520 		return err;
521 
522 	val = tc358746_ps_to_cnt(cfg->clk_trail, hs_byte_clk);
523 	dev_dbg(dev, "TCLK_TRAILCNT: %u (0x%x)\n", val, val);
524 	err = tc358746_write(tc358746, TCLK_TRAILCNT_REG, val);
525 	if (err)
526 		return err;
527 
528 	val = tc358746_ps_to_cnt(cfg->hs_prepare, hs_byte_clk) - 1;
529 	val2 = tc358746_ps_to_cnt(cfg->hs_zero, hs_byte_clk) - 1;
530 	dev_dbg(dev, "THS_PREPARECNT: %u (0x%x)\n", val, val);
531 	dev_dbg(dev, "THS_ZEROCNT: %u (0x%x)\n", val2, val2);
532 	dev_dbg(dev, "THS_HEADERCNT: 0x%x\n",
533 		(u32)(THS_PREPARECNT(val) | THS_ZEROCNT(val2)));
534 	err = tc358746_write(tc358746, THS_HEADERCNT_REG,
535 			     THS_PREPARECNT(val) | THS_ZEROCNT(val2));
536 	if (err)
537 		return err;
538 
539 	/* TWAKEUP > 1ms in lptxcnt steps */
540 	val = tc358746_us_to_cnt(cfg->wakeup, hs_byte_clk);
541 	val = val / (lptxcnt + 1) - 1;
542 	dev_dbg(dev, "TWAKEUP: %u (0x%x)\n", val, val);
543 	err = tc358746_write(tc358746, TWAKEUP_REG, val);
544 	if (err)
545 		return err;
546 
547 	val = tc358746_ps_to_cnt(cfg->clk_post, hs_byte_clk);
548 	dev_dbg(dev, "TCLK_POSTCNT: %u (0x%x)\n", val, val);
549 	err = tc358746_write(tc358746, TCLK_POSTCNT_REG, val);
550 	if (err)
551 		return err;
552 
553 	val = tc358746_ps_to_cnt(cfg->hs_trail, hs_byte_clk);
554 	dev_dbg(dev, "THS_TRAILCNT: %u (0x%x)\n", val, val);
555 	err = tc358746_write(tc358746, THS_TRAILCNT_REG, val);
556 	if (err)
557 		return err;
558 
559 	dev_dbg(dev, "CONTCLKMODE: %u", non_cont_clk ? 0 : 1);
560 
561 	return  tc358746_write(tc358746, TXOPTIONCNTRL_REG, non_cont_clk ? 0 : 1);
562 }
563 
564 #define MAX_DATA_LANES 4
565 
566 static int tc358746_enable_csi_lanes(struct tc358746 *tc358746, int enable)
567 {
568 	unsigned int lanes = tc358746->dphy_cfg.lanes;
569 	unsigned int lane;
570 	u32 reg, val;
571 	int err;
572 
573 	err = tc358746_update_bits(tc358746, CONFCTL_REG, DATALANE_MASK,
574 				   lanes - 1);
575 	if (err)
576 		return err;
577 
578 	/* Clock lane */
579 	val = enable ? 0 : LANEDISABLE;
580 	dev_dbg(tc358746->sd.dev, "CLW_CNTRL: 0x%x\n", val);
581 	err = tc358746_write(tc358746, CLW_CNTRL_REG, val);
582 	if (err)
583 		return err;
584 
585 	for (lane = 0; lane < MAX_DATA_LANES; lane++) {
586 		/* Data lanes */
587 		reg = D0W_CNTRL_REG + lane * 0x4;
588 		val = (enable && lane < lanes) ? 0 : LANEDISABLE;
589 
590 		dev_dbg(tc358746->sd.dev, "D%uW_CNTRL: 0x%x\n", lane, val);
591 		err = tc358746_write(tc358746, reg, val);
592 		if (err)
593 			return err;
594 	}
595 
596 	val = 0;
597 	if (enable) {
598 		/* Clock lane */
599 		val |= BIT(0);
600 
601 		/* Data lanes */
602 		for (lane = 1; lane <= lanes; lane++)
603 			val |= BIT(lane);
604 	}
605 
606 	dev_dbg(tc358746->sd.dev, "HSTXVREGEN: 0x%x\n", val);
607 
608 	return tc358746_write(tc358746, HSTXVREGEN_REG, val);
609 }
610 
611 static int tc358746_enable_csi_module(struct tc358746 *tc358746, int enable)
612 {
613 	unsigned int lanes = tc358746->dphy_cfg.lanes;
614 	int err;
615 
616 	/*
617 	 * START and STRT are only reseted/disabled by sw reset. This is
618 	 * required to put the lane state back into LP-11 state. The sw reset
619 	 * don't reset register values.
620 	 */
621 	if (!enable)
622 		return tc358746_sw_reset(tc358746);
623 
624 	err = tc358746_write(tc358746, STARTCNTRL_REG, START);
625 	if (err)
626 		return err;
627 
628 	err = tc358746_write(tc358746, CSI_START_REG, STRT);
629 	if (err)
630 		return err;
631 
632 	/* CSI_CONTROL_REG is only indirect accessible */
633 	return tc358746_write(tc358746, CSI_CONFW_REG,
634 			      MODE(MODE_SET) |
635 			      ADDRESS(CSI_CONTROL_ADDRESS) |
636 			      DATA(CSI_MODE | TXHSMD | NOL(lanes - 1)));
637 }
638 
639 static int tc358746_enable_parallel_port(struct tc358746 *tc358746, int enable)
640 {
641 	int err;
642 
643 	if (enable) {
644 		err = tc358746_write(tc358746, PP_MISC_REG, 0);
645 		if (err)
646 			return err;
647 
648 		return tc358746_set_bits(tc358746, CONFCTL_REG, PPEN);
649 	}
650 
651 	err = tc358746_set_bits(tc358746, PP_MISC_REG, FRMSTOP);
652 	if (err)
653 		return err;
654 
655 	err = tc358746_clear_bits(tc358746, CONFCTL_REG, PPEN);
656 	if (err)
657 		return err;
658 
659 	return tc358746_set_bits(tc358746, PP_MISC_REG, RSTPTR);
660 }
661 
662 static inline struct v4l2_subdev *tc358746_get_remote_sd(struct media_pad *pad)
663 {
664 	pad = media_pad_remote_pad_first(pad);
665 	if (!pad)
666 		return NULL;
667 
668 	return media_entity_to_v4l2_subdev(pad->entity);
669 }
670 
671 static int tc358746_s_stream(struct v4l2_subdev *sd, int enable)
672 {
673 	struct tc358746 *tc358746 = to_tc358746(sd);
674 	struct v4l2_subdev *src;
675 	int err;
676 
677 	dev_dbg(sd->dev, "%sable\n", enable ? "en" : "dis");
678 
679 	src = tc358746_get_remote_sd(&tc358746->pads[TC358746_SINK]);
680 	if (!src)
681 		return -EPIPE;
682 
683 	if (enable) {
684 		err = pm_runtime_resume_and_get(sd->dev);
685 		if (err)
686 			return err;
687 
688 		err = tc358746_apply_dphy_config(tc358746);
689 		if (err)
690 			goto err_out;
691 
692 		err = tc358746_apply_misc_config(tc358746);
693 		if (err)
694 			goto err_out;
695 
696 		err = tc358746_enable_csi_lanes(tc358746, 1);
697 		if (err)
698 			goto err_out;
699 
700 		err = tc358746_enable_csi_module(tc358746, 1);
701 		if (err)
702 			goto err_out;
703 
704 		err = tc358746_enable_parallel_port(tc358746, 1);
705 		if (err)
706 			goto err_out;
707 
708 		err = v4l2_subdev_call(src, video, s_stream, 1);
709 		if (err)
710 			goto err_out;
711 
712 		return 0;
713 
714 err_out:
715 		pm_runtime_mark_last_busy(sd->dev);
716 		pm_runtime_put_sync_autosuspend(sd->dev);
717 
718 		return err;
719 	}
720 
721 	/*
722 	 * The lanes must be disabled first (before the csi module) so the
723 	 * LP-11 state is entered correctly.
724 	 */
725 	err = tc358746_enable_csi_lanes(tc358746, 0);
726 	if (err)
727 		return err;
728 
729 	err = tc358746_enable_csi_module(tc358746, 0);
730 	if (err)
731 		return err;
732 
733 	err = tc358746_enable_parallel_port(tc358746, 0);
734 	if (err)
735 		return err;
736 
737 	pm_runtime_mark_last_busy(sd->dev);
738 	pm_runtime_put_sync_autosuspend(sd->dev);
739 
740 	return v4l2_subdev_call(src, video, s_stream, 0);
741 }
742 
743 static int tc358746_init_cfg(struct v4l2_subdev *sd,
744 			     struct v4l2_subdev_state *state)
745 {
746 	struct v4l2_mbus_framefmt *fmt;
747 
748 	fmt = v4l2_subdev_get_pad_format(sd, state, TC358746_SINK);
749 	*fmt = tc358746_def_fmt;
750 
751 	fmt = v4l2_subdev_get_pad_format(sd, state, TC358746_SOURCE);
752 	*fmt = tc358746_def_fmt;
753 	fmt->code = tc358746_src_mbus_code(tc358746_def_fmt.code);
754 
755 	return 0;
756 }
757 
758 static int tc358746_enum_mbus_code(struct v4l2_subdev *sd,
759 				   struct v4l2_subdev_state *sd_state,
760 				   struct v4l2_subdev_mbus_code_enum *code)
761 {
762 	const struct tc358746_format *fmt;
763 
764 	fmt = tc358746_get_format_by_idx(code->pad, code->index);
765 	if (IS_ERR(fmt))
766 		return PTR_ERR(fmt);
767 
768 	code->code = fmt->code;
769 
770 	return 0;
771 }
772 
773 static int tc358746_set_fmt(struct v4l2_subdev *sd,
774 			    struct v4l2_subdev_state *sd_state,
775 			    struct v4l2_subdev_format *format)
776 {
777 	struct v4l2_mbus_framefmt *src_fmt, *sink_fmt;
778 	const struct tc358746_format *fmt;
779 
780 	/* Source follows the sink */
781 	if (format->pad == TC358746_SOURCE)
782 		return v4l2_subdev_get_fmt(sd, sd_state, format);
783 
784 	sink_fmt = v4l2_subdev_get_pad_format(sd, sd_state, TC358746_SINK);
785 
786 	fmt = tc358746_get_format_by_code(format->pad, format->format.code);
787 	if (IS_ERR(fmt))
788 		fmt = tc358746_get_format_by_code(format->pad, tc358746_def_fmt.code);
789 
790 	format->format.code = fmt->code;
791 	format->format.field = V4L2_FIELD_NONE;
792 
793 	dev_dbg(sd->dev, "Update format: %ux%u code:0x%x -> %ux%u code:0x%x",
794 		sink_fmt->width, sink_fmt->height, sink_fmt->code,
795 		format->format.width, format->format.height, format->format.code);
796 
797 	*sink_fmt = format->format;
798 
799 	src_fmt = v4l2_subdev_get_pad_format(sd, sd_state, TC358746_SOURCE);
800 	*src_fmt = *sink_fmt;
801 	src_fmt->code = tc358746_src_mbus_code(sink_fmt->code);
802 
803 	return 0;
804 }
805 
806 static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
807 						unsigned long refclk,
808 						unsigned long fout)
809 
810 {
811 	struct device *dev = tc358746->sd.dev;
812 	unsigned long best_freq = 0;
813 	u32 min_delta = 0xffffffff;
814 	u16 prediv_max = 17;
815 	u16 prediv_min = 1;
816 	u16 m_best, mul;
817 	u16 p_best, p;
818 	u8 postdiv;
819 
820 	if (fout > 1000 * HZ_PER_MHZ) {
821 		dev_err(dev, "HS-Clock above 1 Ghz are not supported\n");
822 		return 0;
823 	}
824 
825 	if (fout >= 500 * HZ_PER_MHZ)
826 		postdiv = 1;
827 	else if (fout >= 250 * HZ_PER_MHZ)
828 		postdiv = 2;
829 	else if (fout >= 125 * HZ_PER_MHZ)
830 		postdiv = 4;
831 	else
832 		postdiv = 8;
833 
834 	for (p = prediv_min; p <= prediv_max; p++) {
835 		unsigned long delta, fin;
836 		u64 tmp;
837 
838 		fin = DIV_ROUND_CLOSEST(refclk, p);
839 		if (fin < 4 * HZ_PER_MHZ || fin > 40 * HZ_PER_MHZ)
840 			continue;
841 
842 		tmp = fout * p * postdiv;
843 		do_div(tmp, fin);
844 		mul = tmp;
845 		if (mul > 511)
846 			continue;
847 
848 		tmp = mul * fin;
849 		do_div(tmp, p * postdiv);
850 
851 		delta = abs(fout - tmp);
852 		if (delta < min_delta) {
853 			p_best = p;
854 			m_best = mul;
855 			min_delta = delta;
856 			best_freq = tmp;
857 		};
858 
859 		if (delta == 0)
860 			break;
861 	};
862 
863 	if (!best_freq) {
864 		dev_err(dev, "Failed find PLL frequency\n");
865 		return 0;
866 	}
867 
868 	tc358746->pll_post_div = postdiv;
869 	tc358746->pll_pre_div = p_best;
870 	tc358746->pll_mul = m_best;
871 
872 	if (best_freq != fout)
873 		dev_warn(dev, "Request PLL freq:%lu, found PLL freq:%lu\n",
874 			 fout, best_freq);
875 
876 	dev_dbg(dev, "Found PLL settings: freq:%lu prediv:%u multi:%u postdiv:%u\n",
877 		best_freq, p_best, m_best, postdiv);
878 
879 	return best_freq;
880 }
881 
882 #define TC358746_PRECISION 10
883 
884 static int
885 tc358746_link_validate(struct v4l2_subdev *sd, struct media_link *link,
886 		       struct v4l2_subdev_format *source_fmt,
887 		       struct v4l2_subdev_format *sink_fmt)
888 {
889 	struct tc358746 *tc358746 = to_tc358746(sd);
890 	unsigned long csi_bitrate, source_bitrate;
891 	struct v4l2_subdev_state *sink_state;
892 	struct v4l2_mbus_framefmt *mbusfmt;
893 	const struct tc358746_format *fmt;
894 	unsigned int fifo_sz, tmp, n;
895 	struct v4l2_subdev *source;
896 	s64 source_link_freq;
897 	int err;
898 
899 	err = v4l2_subdev_link_validate_default(sd, link, source_fmt, sink_fmt);
900 	if (err)
901 		return err;
902 
903 	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
904 	mbusfmt = v4l2_subdev_get_pad_format(sd, sink_state, TC358746_SINK);
905 
906 	/* Check the FIFO settings */
907 	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
908 
909 	source = media_entity_to_v4l2_subdev(link->source->entity);
910 	source_link_freq = v4l2_get_link_freq(source->ctrl_handler, 0, 0);
911 	if (source_link_freq <= 0) {
912 		dev_err(tc358746->sd.dev,
913 			"Failed to query or invalid source link frequency\n");
914 		v4l2_subdev_unlock_state(sink_state);
915 		/* Return -EINVAL in case of source_link_freq is 0 */
916 		return source_link_freq ? : -EINVAL;
917 	}
918 	source_bitrate = source_link_freq * fmt->bus_width;
919 
920 	csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
921 
922 	dev_dbg(tc358746->sd.dev,
923 		"Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
924 		source_bitrate, csi_bitrate);
925 
926 	/* Avoid possible FIFO overflows */
927 	if (csi_bitrate < source_bitrate) {
928 		v4l2_subdev_unlock_state(sink_state);
929 		return -EINVAL;
930 	}
931 
932 	/* Best case */
933 	if (csi_bitrate == source_bitrate) {
934 		fifo_sz = TC358746_VB_DEFAULT_SIZE;
935 		tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
936 		goto out;
937 	}
938 
939 	/*
940 	 * Avoid possible FIFO underflow in case of
941 	 * csi_bitrate > source_bitrate. For such case the chip has a internal
942 	 * fifo which can be used to delay the line output.
943 	 *
944 	 * Fifo size calculation (excluding precision):
945 	 *
946 	 * fifo-sz, image-width - in bits
947 	 * sbr                  - source_bitrate in bits/s
948 	 * csir                 - csi_bitrate in bits/s
949 	 *
950 	 * image-width / csir >= (image-width - fifo-sz) / sbr
951 	 * image-width * sbr / csir >= image-width - fifo-sz
952 	 * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
953 	 * fifo-sz >= image-width - image-width / n
954 	 */
955 
956 	source_bitrate /= TC358746_PRECISION;
957 	n = csi_bitrate / source_bitrate;
958 	tmp = (mbusfmt->width * TC358746_PRECISION) / n;
959 	fifo_sz = mbusfmt->width - tmp;
960 	fifo_sz *= fmt->bpp;
961 	tc358746->vb_size = round_up(fifo_sz, 32);
962 
963 out:
964 	dev_dbg(tc358746->sd.dev,
965 		"Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
966 		fifo_sz, tc358746->vb_size);
967 
968 	v4l2_subdev_unlock_state(sink_state);
969 
970 	return tc358746->vb_size > TC358746_VB_MAX_SIZE ? -EINVAL : 0;
971 }
972 
973 static int tc358746_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
974 				    struct v4l2_mbus_config *config)
975 {
976 	struct tc358746 *tc358746 = to_tc358746(sd);
977 
978 	if (pad != TC358746_SOURCE)
979 		return -EINVAL;
980 
981 	config->type = V4L2_MBUS_CSI2_DPHY;
982 	config->bus.mipi_csi2 = tc358746->csi_vep.bus.mipi_csi2;
983 
984 	return 0;
985 }
986 
987 static int __maybe_unused
988 tc358746_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
989 {
990 	struct tc358746 *tc358746 = to_tc358746(sd);
991 
992 	/* 32-bit registers starting from CLW_DPHYCONTTX */
993 	reg->size = reg->reg < CLW_DPHYCONTTX_REG ? 2 : 4;
994 
995 	if (!pm_runtime_get_if_in_use(sd->dev))
996 		return 0;
997 
998 	tc358746_read(tc358746, reg->reg, (u32 *)&reg->val);
999 
1000 	pm_runtime_mark_last_busy(sd->dev);
1001 	pm_runtime_put_sync_autosuspend(sd->dev);
1002 
1003 	return 0;
1004 }
1005 
1006 static int __maybe_unused
1007 tc358746_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
1008 {
1009 	struct tc358746 *tc358746 = to_tc358746(sd);
1010 
1011 	if (!pm_runtime_get_if_in_use(sd->dev))
1012 		return 0;
1013 
1014 	tc358746_write(tc358746, (u32)reg->reg, (u32)reg->val);
1015 
1016 	pm_runtime_mark_last_busy(sd->dev);
1017 	pm_runtime_put_sync_autosuspend(sd->dev);
1018 
1019 	return 0;
1020 }
1021 
1022 static const struct v4l2_subdev_core_ops tc358746_core_ops = {
1023 #ifdef CONFIG_VIDEO_ADV_DEBUG
1024 	.g_register = tc358746_g_register,
1025 	.s_register = tc358746_s_register,
1026 #endif
1027 };
1028 
1029 static const struct v4l2_subdev_video_ops tc358746_video_ops = {
1030 	.s_stream = tc358746_s_stream,
1031 };
1032 
1033 static const struct v4l2_subdev_pad_ops tc358746_pad_ops = {
1034 	.init_cfg = tc358746_init_cfg,
1035 	.enum_mbus_code = tc358746_enum_mbus_code,
1036 	.set_fmt = tc358746_set_fmt,
1037 	.get_fmt = v4l2_subdev_get_fmt,
1038 	.link_validate = tc358746_link_validate,
1039 	.get_mbus_config = tc358746_get_mbus_config,
1040 };
1041 
1042 static const struct v4l2_subdev_ops tc358746_ops = {
1043 	.core = &tc358746_core_ops,
1044 	.video = &tc358746_video_ops,
1045 	.pad = &tc358746_pad_ops,
1046 };
1047 
1048 static const struct media_entity_operations tc358746_entity_ops = {
1049 	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
1050 	.link_validate = v4l2_subdev_link_validate,
1051 };
1052 
1053 static int tc358746_mclk_enable(struct clk_hw *hw)
1054 {
1055 	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1056 	unsigned int div;
1057 	u32 val;
1058 	int err;
1059 
1060 	div = tc358746->mclk_postdiv / 2;
1061 	val = MCLK_HIGH(div - 1) | MCLK_LOW(div - 1);
1062 	dev_dbg(tc358746->sd.dev, "MCLKCTL: %u (0x%x)\n", val, val);
1063 	err = tc358746_write(tc358746, MCLKCTL_REG, val);
1064 	if (err)
1065 		return err;
1066 
1067 	if (tc358746->mclk_prediv == 8)
1068 		val = MCLKDIV(MCLKDIV_8);
1069 	else if (tc358746->mclk_prediv == 4)
1070 		val = MCLKDIV(MCLKDIV_4);
1071 	else
1072 		val = MCLKDIV(MCLKDIV_2);
1073 
1074 	dev_dbg(tc358746->sd.dev, "CLKCTL[MCLKDIV]: %u (0x%x)\n", val, val);
1075 
1076 	return tc358746_update_bits(tc358746, CLKCTL_REG, MCLKDIV_MASK, val);
1077 }
1078 
1079 static void tc358746_mclk_disable(struct clk_hw *hw)
1080 {
1081 	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1082 
1083 	tc358746_write(tc358746, MCLKCTL_REG, 0);
1084 }
1085 
1086 static long
1087 tc358746_find_mclk_settings(struct tc358746 *tc358746, unsigned long mclk_rate)
1088 {
1089 	unsigned long pll_rate = tc358746->pll_rate;
1090 	const unsigned char prediv[] = { 2, 4, 8 };
1091 	unsigned int mclk_prediv, mclk_postdiv;
1092 	struct device *dev = tc358746->sd.dev;
1093 	unsigned int postdiv, mclkdiv;
1094 	unsigned long best_mclk_rate;
1095 	unsigned int i;
1096 
1097 	/*
1098 	 *                          MCLK-Div
1099 	 *           -------------------´`---------------------
1100 	 *          ´                                          `
1101 	 *         +-------------+     +------------------------+
1102 	 *         | MCLK-PreDiv |     |       MCLK-PostDiv     |
1103 	 * PLL --> |   (2/4/8)   | --> | (mclk_low + mclk_high) | --> MCLK
1104 	 *         +-------------+     +------------------------+
1105 	 *
1106 	 * The register value of mclk_low/high is mclk_low/high+1, i.e.:
1107 	 *   mclk_low/high = 1   --> 2 MCLK-Ref Counts
1108 	 *   mclk_low/high = 255 --> 256 MCLK-Ref Counts == max.
1109 	 * If mclk_low and mclk_high are 0 then MCLK is disabled.
1110 	 *
1111 	 * Keep it simple and support 50/50 duty cycles only for now,
1112 	 * so the calc will be:
1113 	 *
1114 	 *   MCLK = PLL / (MCLK-PreDiv * 2 * MCLK-PostDiv)
1115 	 */
1116 
1117 	if (mclk_rate == tc358746->mclk_rate)
1118 		return mclk_rate;
1119 
1120 	/* Highest possible rate */
1121 	mclkdiv = pll_rate / mclk_rate;
1122 	if (mclkdiv <= 8) {
1123 		mclk_prediv = 2;
1124 		mclk_postdiv = 4;
1125 		best_mclk_rate = pll_rate / (2 * 4);
1126 		goto out;
1127 	}
1128 
1129 	/* First check the prediv */
1130 	for (i = 0; i < ARRAY_SIZE(prediv); i++) {
1131 		postdiv = mclkdiv / prediv[i];
1132 
1133 		if (postdiv % 2)
1134 			continue;
1135 
1136 		if (postdiv >= 4 && postdiv <= 512) {
1137 			mclk_prediv = prediv[i];
1138 			mclk_postdiv = postdiv;
1139 			best_mclk_rate = pll_rate / (prediv[i] * postdiv);
1140 			goto out;
1141 		}
1142 	}
1143 
1144 	/* No suitable prediv found, so try to adjust the postdiv */
1145 	for (postdiv = 4; postdiv <= 512; postdiv += 2) {
1146 		unsigned int pre;
1147 
1148 		pre = mclkdiv / postdiv;
1149 		if (pre == 2 || pre == 4 || pre == 8) {
1150 			mclk_prediv = pre;
1151 			mclk_postdiv = postdiv;
1152 			best_mclk_rate = pll_rate / (pre * postdiv);
1153 			goto out;
1154 		}
1155 	}
1156 
1157 	/* The MCLK <-> PLL gap is to high -> use largest possible div */
1158 	mclk_prediv = 8;
1159 	mclk_postdiv = 512;
1160 	best_mclk_rate = pll_rate / (8 * 512);
1161 
1162 out:
1163 	tc358746->mclk_prediv = mclk_prediv;
1164 	tc358746->mclk_postdiv = mclk_postdiv;
1165 	tc358746->mclk_rate = best_mclk_rate;
1166 
1167 	if (best_mclk_rate != mclk_rate)
1168 		dev_warn(dev, "Request MCLK freq:%lu, found MCLK freq:%lu\n",
1169 			 mclk_rate, best_mclk_rate);
1170 
1171 	dev_dbg(dev, "Found MCLK settings: freq:%lu prediv:%u postdiv:%u\n",
1172 		best_mclk_rate, mclk_prediv, mclk_postdiv);
1173 
1174 	return best_mclk_rate;
1175 }
1176 
1177 static unsigned long
1178 tc358746_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1179 {
1180 	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1181 	unsigned int prediv, postdiv;
1182 	u32 val;
1183 	int err;
1184 
1185 	err = tc358746_read(tc358746, MCLKCTL_REG, &val);
1186 	if (err)
1187 		return 0;
1188 
1189 	postdiv = FIELD_GET(MCLK_LOW_MASK, val) + 1;
1190 	postdiv += FIELD_GET(MCLK_HIGH_MASK, val) + 1;
1191 
1192 	err = tc358746_read(tc358746, CLKCTL_REG, &val);
1193 	if (err)
1194 		return 0;
1195 
1196 	prediv = FIELD_GET(MCLKDIV_MASK, val);
1197 	if (prediv == MCLKDIV_8)
1198 		prediv = 8;
1199 	else if (prediv == MCLKDIV_4)
1200 		prediv = 4;
1201 	else
1202 		prediv = 2;
1203 
1204 	return tc358746->pll_rate / (prediv * postdiv);
1205 }
1206 
1207 static long tc358746_mclk_round_rate(struct clk_hw *hw, unsigned long rate,
1208 				     unsigned long *parent_rate)
1209 {
1210 	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1211 
1212 	*parent_rate = tc358746->pll_rate;
1213 
1214 	return tc358746_find_mclk_settings(tc358746, rate);
1215 }
1216 
1217 static int tc358746_mclk_set_rate(struct clk_hw *hw, unsigned long rate,
1218 				  unsigned long parent_rate)
1219 {
1220 	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1221 
1222 	tc358746_find_mclk_settings(tc358746, rate);
1223 
1224 	return tc358746_mclk_enable(hw);
1225 }
1226 
1227 static const struct clk_ops tc358746_mclk_ops = {
1228 	.enable = tc358746_mclk_enable,
1229 	.disable = tc358746_mclk_disable,
1230 	.recalc_rate = tc358746_recalc_rate,
1231 	.round_rate = tc358746_mclk_round_rate,
1232 	.set_rate = tc358746_mclk_set_rate,
1233 };
1234 
1235 static int tc358746_setup_mclk_provider(struct tc358746 *tc358746)
1236 {
1237 	struct clk_init_data mclk_initdata = { };
1238 	struct device *dev = tc358746->sd.dev;
1239 	const char *mclk_name;
1240 	int err;
1241 
1242 	/* MCLK clk provider support is optional */
1243 	if (!device_property_present(dev, "#clock-cells"))
1244 		return 0;
1245 
1246 	/* Init to highest possibel MCLK */
1247 	tc358746->mclk_postdiv = 512;
1248 	tc358746->mclk_prediv = 8;
1249 
1250 	mclk_name = "tc358746-mclk";
1251 	device_property_read_string(dev, "clock-output-names", &mclk_name);
1252 
1253 	mclk_initdata.name = mclk_name;
1254 	mclk_initdata.ops = &tc358746_mclk_ops;
1255 	tc358746->mclk_hw.init = &mclk_initdata;
1256 
1257 	err = devm_clk_hw_register(dev, &tc358746->mclk_hw);
1258 	if (err) {
1259 		dev_err(dev, "Failed to register mclk provider\n");
1260 		return err;
1261 	}
1262 
1263 	err = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1264 					  &tc358746->mclk_hw);
1265 	if (err)
1266 		dev_err(dev, "Failed to add mclk provider\n");
1267 
1268 	return err;
1269 }
1270 
1271 static int
1272 tc358746_init_subdev(struct tc358746 *tc358746, struct i2c_client *client)
1273 {
1274 	struct v4l2_subdev *sd = &tc358746->sd;
1275 	int err;
1276 
1277 	v4l2_i2c_subdev_init(sd, client, &tc358746_ops);
1278 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1279 	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1280 	sd->entity.ops = &tc358746_entity_ops;
1281 
1282 	tc358746->pads[TC358746_SINK].flags = MEDIA_PAD_FL_SINK;
1283 	tc358746->pads[TC358746_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1284 	err = media_entity_pads_init(&sd->entity, TC358746_NR_PADS,
1285 				     tc358746->pads);
1286 	if (err)
1287 		return err;
1288 
1289 	err = v4l2_subdev_init_finalize(sd);
1290 	if (err)
1291 		media_entity_cleanup(&sd->entity);
1292 
1293 	return err;
1294 }
1295 
1296 static int
1297 tc358746_init_output_port(struct tc358746 *tc358746, unsigned long refclk)
1298 {
1299 	struct device *dev = tc358746->sd.dev;
1300 	struct v4l2_fwnode_endpoint *vep;
1301 	unsigned long csi_link_rate;
1302 	struct fwnode_handle *ep;
1303 	unsigned char csi_lanes;
1304 	int err;
1305 
1306 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), TC358746_SOURCE,
1307 					     0, 0);
1308 	if (!ep) {
1309 		dev_err(dev, "Missing endpoint node\n");
1310 		return -EINVAL;
1311 	}
1312 
1313 	/* Currently we only support 'parallel in' -> 'csi out' */
1314 	vep = &tc358746->csi_vep;
1315 	vep->bus_type = V4L2_MBUS_CSI2_DPHY;
1316 	err = v4l2_fwnode_endpoint_alloc_parse(ep, vep);
1317 	fwnode_handle_put(ep);
1318 	if (err) {
1319 		dev_err(dev, "Failed to parse source endpoint\n");
1320 		return err;
1321 	}
1322 
1323 	csi_lanes = vep->bus.mipi_csi2.num_data_lanes;
1324 	if (csi_lanes == 0 || csi_lanes > 4 ||
1325 	    vep->nr_of_link_frequencies == 0) {
1326 		dev_err(dev, "error: Invalid CSI-2 settings\n");
1327 		err = -EINVAL;
1328 		goto err;
1329 	}
1330 
1331 	/* TODO: Add support to handle multiple link frequencies */
1332 	csi_link_rate = (unsigned long)vep->link_frequencies[0];
1333 	tc358746->pll_rate = tc358746_find_pll_settings(tc358746, refclk,
1334 							csi_link_rate * 2);
1335 	if (!tc358746->pll_rate) {
1336 		err = -EINVAL;
1337 		goto err;
1338 	}
1339 
1340 	err = phy_mipi_dphy_get_default_config_for_hsclk(tc358746->pll_rate,
1341 						csi_lanes, &tc358746->dphy_cfg);
1342 	if (err)
1343 		goto err;
1344 
1345 	tc358746->vb_size = TC358746_VB_DEFAULT_SIZE;
1346 
1347 	return 0;
1348 
1349 err:
1350 	v4l2_fwnode_endpoint_free(vep);
1351 
1352 	return err;
1353 }
1354 
1355 static int tc358746_init_hw(struct tc358746 *tc358746)
1356 {
1357 	struct device *dev = tc358746->sd.dev;
1358 	unsigned int chipid;
1359 	u32 val;
1360 	int err;
1361 
1362 	err = pm_runtime_resume_and_get(dev);
1363 	if (err < 0) {
1364 		dev_err(dev, "Failed to resume the device\n");
1365 		return err;
1366 	}
1367 
1368 	 /* Ensure that CSI interface is put into LP-11 state */
1369 	err = tc358746_sw_reset(tc358746);
1370 	if (err) {
1371 		pm_runtime_put_sync(dev);
1372 		dev_err(dev, "Failed to reset the device\n");
1373 		return err;
1374 	}
1375 
1376 	err = tc358746_read(tc358746, CHIPID_REG, &val);
1377 	pm_runtime_mark_last_busy(dev);
1378 	pm_runtime_put_sync_autosuspend(dev);
1379 	if (err)
1380 		return -ENODEV;
1381 
1382 	chipid = FIELD_GET(CHIPID, val);
1383 	if (chipid != 0x44) {
1384 		dev_err(dev, "Invalid chipid 0x%02x\n", chipid);
1385 		return -ENODEV;
1386 	}
1387 
1388 	return 0;
1389 }
1390 
1391 static int tc358746_init_controls(struct tc358746 *tc358746)
1392 {
1393 	u64 *link_frequencies = tc358746->csi_vep.link_frequencies;
1394 	struct v4l2_ctrl *ctrl;
1395 	int err;
1396 
1397 	err = v4l2_ctrl_handler_init(&tc358746->ctrl_hdl, 1);
1398 	if (err)
1399 		return err;
1400 
1401 	/*
1402 	 * The driver currently supports only one link-frequency, regardless of
1403 	 * the input from the firmware, see: tc358746_init_output_port(). So
1404 	 * report only the first frequency from the array of possible given
1405 	 * frequencies.
1406 	 */
1407 	ctrl = v4l2_ctrl_new_int_menu(&tc358746->ctrl_hdl, NULL,
1408 				      V4L2_CID_LINK_FREQ, 0, 0,
1409 				      link_frequencies);
1410 	if (ctrl)
1411 		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1412 
1413 	err = tc358746->ctrl_hdl.error;
1414 	if (err) {
1415 		v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1416 		return err;
1417 	}
1418 
1419 	tc358746->sd.ctrl_handler = &tc358746->ctrl_hdl;
1420 
1421 	return 0;
1422 }
1423 
1424 static int tc358746_notify_bound(struct v4l2_async_notifier *notifier,
1425 				 struct v4l2_subdev *sd,
1426 				 struct v4l2_async_subdev *asd)
1427 {
1428 	struct tc358746 *tc358746 =
1429 		container_of(notifier, struct tc358746, notifier);
1430 	u32 flags = MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1431 	struct media_pad *sink = &tc358746->pads[TC358746_SINK];
1432 
1433 	return v4l2_create_fwnode_links_to_pad(sd, sink, flags);
1434 }
1435 
1436 static const struct v4l2_async_notifier_operations tc358746_notify_ops = {
1437 	.bound = tc358746_notify_bound,
1438 };
1439 
1440 static int tc358746_async_register(struct tc358746 *tc358746)
1441 {
1442 	struct v4l2_fwnode_endpoint vep = {
1443 		.bus_type = V4L2_MBUS_PARALLEL,
1444 	};
1445 	struct v4l2_async_subdev *asd;
1446 	struct fwnode_handle *ep;
1447 	int err;
1448 
1449 	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(tc358746->sd.dev),
1450 					     TC358746_SINK, 0, 0);
1451 	if (!ep)
1452 		return -ENOTCONN;
1453 
1454 	err = v4l2_fwnode_endpoint_parse(ep, &vep);
1455 	if (err) {
1456 		fwnode_handle_put(ep);
1457 		return err;
1458 	}
1459 
1460 	v4l2_async_nf_init(&tc358746->notifier);
1461 	asd = v4l2_async_nf_add_fwnode_remote(&tc358746->notifier, ep,
1462 					      struct v4l2_async_subdev);
1463 	fwnode_handle_put(ep);
1464 
1465 	if (IS_ERR(asd)) {
1466 		err = PTR_ERR(asd);
1467 		goto err_cleanup;
1468 	}
1469 
1470 	tc358746->notifier.ops = &tc358746_notify_ops;
1471 
1472 	err = v4l2_async_subdev_nf_register(&tc358746->sd, &tc358746->notifier);
1473 	if (err)
1474 		goto err_cleanup;
1475 
1476 	tc358746->sd.fwnode = fwnode_graph_get_endpoint_by_id(
1477 		dev_fwnode(tc358746->sd.dev), TC358746_SOURCE, 0, 0);
1478 
1479 	err = v4l2_async_register_subdev(&tc358746->sd);
1480 	if (err)
1481 		goto err_unregister;
1482 
1483 	return 0;
1484 
1485 err_unregister:
1486 	fwnode_handle_put(tc358746->sd.fwnode);
1487 	v4l2_async_nf_unregister(&tc358746->notifier);
1488 err_cleanup:
1489 	v4l2_async_nf_cleanup(&tc358746->notifier);
1490 
1491 	return err;
1492 }
1493 
1494 static int tc358746_probe(struct i2c_client *client)
1495 {
1496 	struct device *dev = &client->dev;
1497 	struct tc358746 *tc358746;
1498 	unsigned long refclk;
1499 	unsigned int i;
1500 	int err;
1501 
1502 	tc358746 = devm_kzalloc(&client->dev, sizeof(*tc358746), GFP_KERNEL);
1503 	if (!tc358746)
1504 		return -ENOMEM;
1505 
1506 	tc358746->regmap = devm_regmap_init_i2c(client, &tc358746_regmap_config);
1507 	if (IS_ERR(tc358746->regmap))
1508 		return dev_err_probe(dev, PTR_ERR(tc358746->regmap),
1509 				     "Failed to init regmap\n");
1510 
1511 	tc358746->refclk = devm_clk_get(dev, "refclk");
1512 	if (IS_ERR(tc358746->refclk))
1513 		return dev_err_probe(dev, PTR_ERR(tc358746->refclk),
1514 				     "Failed to get refclk\n");
1515 
1516 	err = clk_prepare_enable(tc358746->refclk);
1517 	if (err)
1518 		return dev_err_probe(dev, err,
1519 				     "Failed to enable refclk\n");
1520 
1521 	refclk = clk_get_rate(tc358746->refclk);
1522 	clk_disable_unprepare(tc358746->refclk);
1523 
1524 	if (refclk < 6 * HZ_PER_MHZ || refclk > 40 * HZ_PER_MHZ)
1525 		return dev_err_probe(dev, -EINVAL, "Invalid refclk range\n");
1526 
1527 	for (i = 0; i < ARRAY_SIZE(tc358746_supplies); i++)
1528 		tc358746->supplies[i].supply = tc358746_supplies[i];
1529 
1530 	err = devm_regulator_bulk_get(dev, ARRAY_SIZE(tc358746_supplies),
1531 				      tc358746->supplies);
1532 	if (err)
1533 		return dev_err_probe(dev, err, "Failed to get supplies\n");
1534 
1535 	tc358746->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1536 						       GPIOD_OUT_HIGH);
1537 	if (IS_ERR(tc358746->reset_gpio))
1538 		return dev_err_probe(dev, PTR_ERR(tc358746->reset_gpio),
1539 				     "Failed to get reset-gpios\n");
1540 
1541 	err = tc358746_init_subdev(tc358746, client);
1542 	if (err)
1543 		return dev_err_probe(dev, err, "Failed to init subdev\n");
1544 
1545 	err = tc358746_init_output_port(tc358746, refclk);
1546 	if (err)
1547 		goto err_subdev;
1548 
1549 	/*
1550 	 * Keep this order since we need the output port link-frequencies
1551 	 * information.
1552 	 */
1553 	err = tc358746_init_controls(tc358746);
1554 	if (err)
1555 		goto err_fwnode;
1556 
1557 	dev_set_drvdata(dev, tc358746);
1558 
1559 	/* Set to 1sec to give the stream reconfiguration enough time */
1560 	pm_runtime_set_autosuspend_delay(dev, 1000);
1561 	pm_runtime_use_autosuspend(dev);
1562 	pm_runtime_enable(dev);
1563 
1564 	err = tc358746_init_hw(tc358746);
1565 	if (err)
1566 		goto err_pm;
1567 
1568 	err = tc358746_setup_mclk_provider(tc358746);
1569 	if (err)
1570 		goto err_pm;
1571 
1572 	err = tc358746_async_register(tc358746);
1573 	if (err < 0)
1574 		goto err_pm;
1575 
1576 	dev_dbg(dev, "%s found @ 0x%x (%s)\n", client->name,
1577 		client->addr, client->adapter->name);
1578 
1579 	return 0;
1580 
1581 err_pm:
1582 	pm_runtime_disable(dev);
1583 	pm_runtime_set_suspended(dev);
1584 	pm_runtime_dont_use_autosuspend(dev);
1585 	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1586 err_fwnode:
1587 	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1588 err_subdev:
1589 	v4l2_subdev_cleanup(&tc358746->sd);
1590 	media_entity_cleanup(&tc358746->sd.entity);
1591 
1592 	return err;
1593 }
1594 
1595 static void tc358746_remove(struct i2c_client *client)
1596 {
1597 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1598 	struct tc358746 *tc358746 = to_tc358746(sd);
1599 
1600 	v4l2_subdev_cleanup(sd);
1601 	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1602 	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1603 	v4l2_async_nf_unregister(&tc358746->notifier);
1604 	v4l2_async_nf_cleanup(&tc358746->notifier);
1605 	fwnode_handle_put(sd->fwnode);
1606 	v4l2_async_unregister_subdev(sd);
1607 	media_entity_cleanup(&sd->entity);
1608 
1609 	pm_runtime_disable(sd->dev);
1610 	pm_runtime_set_suspended(sd->dev);
1611 	pm_runtime_dont_use_autosuspend(sd->dev);
1612 }
1613 
1614 static int tc358746_suspend(struct device *dev)
1615 {
1616 	struct tc358746 *tc358746 = dev_get_drvdata(dev);
1617 	int err;
1618 
1619 	clk_disable_unprepare(tc358746->refclk);
1620 
1621 	err = regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1622 				     tc358746->supplies);
1623 	if (err)
1624 		clk_prepare_enable(tc358746->refclk);
1625 
1626 	return err;
1627 }
1628 
1629 static int tc358746_resume(struct device *dev)
1630 {
1631 	struct tc358746 *tc358746 = dev_get_drvdata(dev);
1632 	int err;
1633 
1634 	gpiod_set_value(tc358746->reset_gpio, 1);
1635 
1636 	err = regulator_bulk_enable(ARRAY_SIZE(tc358746_supplies),
1637 				    tc358746->supplies);
1638 	if (err)
1639 		return err;
1640 
1641 	/* min. 200ns */
1642 	usleep_range(10, 20);
1643 
1644 	gpiod_set_value(tc358746->reset_gpio, 0);
1645 
1646 	err = clk_prepare_enable(tc358746->refclk);
1647 	if (err)
1648 		goto err;
1649 
1650 	/* min. 700us ... 1ms */
1651 	usleep_range(1000, 1500);
1652 
1653 	/*
1654 	 * Enable the PLL here since it can be called by the clk-framework or by
1655 	 * the .s_stream() callback. So this is the common place for both.
1656 	 */
1657 	err = tc358746_apply_pll_config(tc358746);
1658 	if (err)
1659 		goto err_clk;
1660 
1661 	return 0;
1662 
1663 err_clk:
1664 	clk_disable_unprepare(tc358746->refclk);
1665 err:
1666 	regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1667 			       tc358746->supplies);
1668 	return err;
1669 }
1670 
1671 static DEFINE_RUNTIME_DEV_PM_OPS(tc358746_pm_ops, tc358746_suspend,
1672 				 tc358746_resume, NULL);
1673 
1674 static const struct of_device_id __maybe_unused tc358746_of_match[] = {
1675 	{ .compatible = "toshiba,tc358746" },
1676 	{ },
1677 };
1678 MODULE_DEVICE_TABLE(of, tc358746_of_match);
1679 
1680 static struct i2c_driver tc358746_driver = {
1681 	.driver = {
1682 		.name = "tc358746",
1683 		.pm = pm_ptr(&tc358746_pm_ops),
1684 		.of_match_table = tc358746_of_match,
1685 	},
1686 	.probe_new = tc358746_probe,
1687 	.remove = tc358746_remove,
1688 };
1689 
1690 module_i2c_driver(tc358746_driver);
1691 
1692 MODULE_DESCRIPTION("Toshiba TC358746 Parallel to CSI-2 bridge driver");
1693 MODULE_AUTHOR("Marco Felsch <kernel@pengutronix.de>");
1694 MODULE_LICENSE("GPL");
1695