xref: /openbmc/linux/drivers/media/i2c/st-mipid02.c (revision 4a075bd4)
1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Driver for ST MIPID02 CSI-2 to PARALLEL bridge
4  *
5  * Copyright (C) STMicroelectronics SA 2019
6  * Authors: Mickael Guene <mickael.guene@st.com>
7  *          for STMicroelectronics.
8  *
9  *
10  */
11 
12 #include <linux/clk.h>
13 #include <linux/delay.h>
14 #include <linux/gpio/consumer.h>
15 #include <linux/i2c.h>
16 #include <linux/module.h>
17 #include <linux/of_graph.h>
18 #include <linux/regulator/consumer.h>
19 #include <media/v4l2-async.h>
20 #include <media/v4l2-ctrls.h>
21 #include <media/v4l2-device.h>
22 #include <media/v4l2-fwnode.h>
23 #include <media/v4l2-subdev.h>
24 
25 #define MIPID02_CLK_LANE_WR_REG1			0x01
26 #define MIPID02_CLK_LANE_REG1				0x02
27 #define MIPID02_CLK_LANE_REG3				0x04
28 #define MIPID02_DATA_LANE0_REG1				0x05
29 #define MIPID02_DATA_LANE0_REG2				0x06
30 #define MIPID02_DATA_LANE1_REG1				0x09
31 #define MIPID02_DATA_LANE1_REG2				0x0a
32 #define MIPID02_MODE_REG1				0x14
33 #define MIPID02_MODE_REG2				0x15
34 #define MIPID02_DATA_ID_RREG				0x17
35 #define MIPID02_DATA_SELECTION_CTRL			0x19
36 #define MIPID02_PIX_WIDTH_CTRL				0x1e
37 #define MIPID02_PIX_WIDTH_CTRL_EMB			0x1f
38 
39 /* Bits definition for MIPID02_CLK_LANE_REG1 */
40 #define CLK_ENABLE					BIT(0)
41 /* Bits definition for MIPID02_CLK_LANE_REG3 */
42 #define CLK_MIPI_CSI					BIT(1)
43 /* Bits definition for MIPID02_DATA_LANE0_REG1 */
44 #define DATA_ENABLE					BIT(0)
45 /* Bits definition for MIPID02_DATA_LANEx_REG2 */
46 #define DATA_MIPI_CSI					BIT(0)
47 /* Bits definition for MIPID02_MODE_REG1 */
48 #define MODE_DATA_SWAP					BIT(2)
49 #define MODE_NO_BYPASS					BIT(6)
50 /* Bits definition for MIPID02_MODE_REG2 */
51 #define MODE_HSYNC_ACTIVE_HIGH				BIT(1)
52 #define MODE_VSYNC_ACTIVE_HIGH				BIT(2)
53 /* Bits definition for MIPID02_DATA_SELECTION_CTRL */
54 #define SELECTION_MANUAL_DATA				BIT(2)
55 #define SELECTION_MANUAL_WIDTH				BIT(3)
56 
57 static const u32 mipid02_supported_fmt_codes[] = {
58 	MEDIA_BUS_FMT_SBGGR8_1X8, MEDIA_BUS_FMT_SGBRG8_1X8,
59 	MEDIA_BUS_FMT_SGRBG8_1X8, MEDIA_BUS_FMT_SRGGB8_1X8,
60 	MEDIA_BUS_FMT_SBGGR10_1X10, MEDIA_BUS_FMT_SGBRG10_1X10,
61 	MEDIA_BUS_FMT_SGRBG10_1X10, MEDIA_BUS_FMT_SRGGB10_1X10,
62 	MEDIA_BUS_FMT_SBGGR12_1X12, MEDIA_BUS_FMT_SGBRG12_1X12,
63 	MEDIA_BUS_FMT_SGRBG12_1X12, MEDIA_BUS_FMT_SRGGB12_1X12,
64 	MEDIA_BUS_FMT_UYVY8_1X16, MEDIA_BUS_FMT_BGR888_1X24
65 };
66 
67 /* regulator supplies */
68 static const char * const mipid02_supply_name[] = {
69 	"VDDE", /* 1.8V digital I/O supply */
70 	"VDDIN", /* 1V8 voltage regulator supply */
71 };
72 
73 #define MIPID02_NUM_SUPPLIES		ARRAY_SIZE(mipid02_supply_name)
74 
75 #define MIPID02_SINK_0			0
76 #define MIPID02_SINK_1			1
77 #define MIPID02_SOURCE			2
78 #define MIPID02_PAD_NB			3
79 
80 struct mipid02_dev {
81 	struct i2c_client *i2c_client;
82 	struct regulator_bulk_data supplies[MIPID02_NUM_SUPPLIES];
83 	struct v4l2_subdev sd;
84 	struct media_pad pad[MIPID02_PAD_NB];
85 	struct clk *xclk;
86 	struct gpio_desc *reset_gpio;
87 	/* endpoints info */
88 	struct v4l2_fwnode_endpoint rx;
89 	u64 link_frequency;
90 	struct v4l2_fwnode_endpoint tx;
91 	/* remote source */
92 	struct v4l2_async_subdev asd;
93 	struct v4l2_async_notifier notifier;
94 	struct v4l2_subdev *s_subdev;
95 	/* registers */
96 	struct {
97 		u8 clk_lane_reg1;
98 		u8 data_lane0_reg1;
99 		u8 data_lane1_reg1;
100 		u8 mode_reg1;
101 		u8 mode_reg2;
102 		u8 data_id_rreg;
103 		u8 pix_width_ctrl;
104 		u8 pix_width_ctrl_emb;
105 	} r;
106 	/* lock to protect all members below */
107 	struct mutex lock;
108 	bool streaming;
109 	struct v4l2_mbus_framefmt fmt;
110 };
111 
112 static int bpp_from_code(__u32 code)
113 {
114 	switch (code) {
115 	case MEDIA_BUS_FMT_SBGGR8_1X8:
116 	case MEDIA_BUS_FMT_SGBRG8_1X8:
117 	case MEDIA_BUS_FMT_SGRBG8_1X8:
118 	case MEDIA_BUS_FMT_SRGGB8_1X8:
119 		return 8;
120 	case MEDIA_BUS_FMT_SBGGR10_1X10:
121 	case MEDIA_BUS_FMT_SGBRG10_1X10:
122 	case MEDIA_BUS_FMT_SGRBG10_1X10:
123 	case MEDIA_BUS_FMT_SRGGB10_1X10:
124 		return 10;
125 	case MEDIA_BUS_FMT_SBGGR12_1X12:
126 	case MEDIA_BUS_FMT_SGBRG12_1X12:
127 	case MEDIA_BUS_FMT_SGRBG12_1X12:
128 	case MEDIA_BUS_FMT_SRGGB12_1X12:
129 		return 12;
130 	case MEDIA_BUS_FMT_UYVY8_1X16:
131 		return 16;
132 	case MEDIA_BUS_FMT_BGR888_1X24:
133 		return 24;
134 	default:
135 		return 0;
136 	}
137 }
138 
139 static u8 data_type_from_code(__u32 code)
140 {
141 	switch (code) {
142 	case MEDIA_BUS_FMT_SBGGR8_1X8:
143 	case MEDIA_BUS_FMT_SGBRG8_1X8:
144 	case MEDIA_BUS_FMT_SGRBG8_1X8:
145 	case MEDIA_BUS_FMT_SRGGB8_1X8:
146 		return 0x2a;
147 	case MEDIA_BUS_FMT_SBGGR10_1X10:
148 	case MEDIA_BUS_FMT_SGBRG10_1X10:
149 	case MEDIA_BUS_FMT_SGRBG10_1X10:
150 	case MEDIA_BUS_FMT_SRGGB10_1X10:
151 		return 0x2b;
152 	case MEDIA_BUS_FMT_SBGGR12_1X12:
153 	case MEDIA_BUS_FMT_SGBRG12_1X12:
154 	case MEDIA_BUS_FMT_SGRBG12_1X12:
155 	case MEDIA_BUS_FMT_SRGGB12_1X12:
156 		return 0x2c;
157 	case MEDIA_BUS_FMT_UYVY8_1X16:
158 		return 0x1e;
159 	case MEDIA_BUS_FMT_BGR888_1X24:
160 		return 0x24;
161 	default:
162 		return 0;
163 	}
164 }
165 
166 static void init_format(struct v4l2_mbus_framefmt *fmt)
167 {
168 	fmt->code = MEDIA_BUS_FMT_SBGGR8_1X8;
169 	fmt->field = V4L2_FIELD_NONE;
170 	fmt->colorspace = V4L2_COLORSPACE_SRGB;
171 	fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(V4L2_COLORSPACE_SRGB);
172 	fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
173 	fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(V4L2_COLORSPACE_SRGB);
174 	fmt->width = 640;
175 	fmt->height = 480;
176 }
177 
178 static __u32 get_fmt_code(__u32 code)
179 {
180 	unsigned int i;
181 
182 	for (i = 0; i < ARRAY_SIZE(mipid02_supported_fmt_codes); i++) {
183 		if (code == mipid02_supported_fmt_codes[i])
184 			return code;
185 	}
186 
187 	return mipid02_supported_fmt_codes[0];
188 }
189 
190 static __u32 serial_to_parallel_code(__u32 serial)
191 {
192 	if (serial == MEDIA_BUS_FMT_UYVY8_1X16)
193 		return MEDIA_BUS_FMT_UYVY8_2X8;
194 	if (serial == MEDIA_BUS_FMT_BGR888_1X24)
195 		return MEDIA_BUS_FMT_BGR888_3X8;
196 
197 	return serial;
198 }
199 
200 static inline struct mipid02_dev *to_mipid02_dev(struct v4l2_subdev *sd)
201 {
202 	return container_of(sd, struct mipid02_dev, sd);
203 }
204 
205 static int mipid02_read_reg(struct mipid02_dev *bridge, u16 reg, u8 *val)
206 {
207 	struct i2c_client *client = bridge->i2c_client;
208 	struct i2c_msg msg[2];
209 	u8 buf[2];
210 	int ret;
211 
212 	buf[0] = reg >> 8;
213 	buf[1] = reg & 0xff;
214 
215 	msg[0].addr = client->addr;
216 	msg[0].flags = client->flags;
217 	msg[0].buf = buf;
218 	msg[0].len = sizeof(buf);
219 
220 	msg[1].addr = client->addr;
221 	msg[1].flags = client->flags | I2C_M_RD;
222 	msg[1].buf = val;
223 	msg[1].len = 1;
224 
225 	ret = i2c_transfer(client->adapter, msg, 2);
226 	if (ret < 0) {
227 		dev_dbg(&client->dev, "%s: %x i2c_transfer, reg: %x => %d\n",
228 			    __func__, client->addr, reg, ret);
229 		return ret;
230 	}
231 
232 	return 0;
233 }
234 
235 static int mipid02_write_reg(struct mipid02_dev *bridge, u16 reg, u8 val)
236 {
237 	struct i2c_client *client = bridge->i2c_client;
238 	struct i2c_msg msg;
239 	u8 buf[3];
240 	int ret;
241 
242 	buf[0] = reg >> 8;
243 	buf[1] = reg & 0xff;
244 	buf[2] = val;
245 
246 	msg.addr = client->addr;
247 	msg.flags = client->flags;
248 	msg.buf = buf;
249 	msg.len = sizeof(buf);
250 
251 	ret = i2c_transfer(client->adapter, &msg, 1);
252 	if (ret < 0) {
253 		dev_dbg(&client->dev, "%s: i2c_transfer, reg: %x => %d\n",
254 			    __func__, reg, ret);
255 		return ret;
256 	}
257 
258 	return 0;
259 }
260 
261 static int mipid02_get_regulators(struct mipid02_dev *bridge)
262 {
263 	unsigned int i;
264 
265 	for (i = 0; i < MIPID02_NUM_SUPPLIES; i++)
266 		bridge->supplies[i].supply = mipid02_supply_name[i];
267 
268 	return devm_regulator_bulk_get(&bridge->i2c_client->dev,
269 				       MIPID02_NUM_SUPPLIES,
270 				       bridge->supplies);
271 }
272 
273 static void mipid02_apply_reset(struct mipid02_dev *bridge)
274 {
275 	gpiod_set_value_cansleep(bridge->reset_gpio, 0);
276 	usleep_range(5000, 10000);
277 	gpiod_set_value_cansleep(bridge->reset_gpio, 1);
278 	usleep_range(5000, 10000);
279 	gpiod_set_value_cansleep(bridge->reset_gpio, 0);
280 	usleep_range(5000, 10000);
281 }
282 
283 static int mipid02_set_power_on(struct mipid02_dev *bridge)
284 {
285 	struct i2c_client *client = bridge->i2c_client;
286 	int ret;
287 
288 	ret = clk_prepare_enable(bridge->xclk);
289 	if (ret) {
290 		dev_err(&client->dev, "%s: failed to enable clock\n", __func__);
291 		return ret;
292 	}
293 
294 	ret = regulator_bulk_enable(MIPID02_NUM_SUPPLIES,
295 				    bridge->supplies);
296 	if (ret) {
297 		dev_err(&client->dev, "%s: failed to enable regulators\n",
298 			    __func__);
299 		goto xclk_off;
300 	}
301 
302 	if (bridge->reset_gpio) {
303 		dev_dbg(&client->dev, "apply reset");
304 		mipid02_apply_reset(bridge);
305 	} else {
306 		dev_dbg(&client->dev, "don't apply reset");
307 		usleep_range(5000, 10000);
308 	}
309 
310 	return 0;
311 
312 xclk_off:
313 	clk_disable_unprepare(bridge->xclk);
314 	return ret;
315 }
316 
317 static void mipid02_set_power_off(struct mipid02_dev *bridge)
318 {
319 	regulator_bulk_disable(MIPID02_NUM_SUPPLIES, bridge->supplies);
320 	clk_disable_unprepare(bridge->xclk);
321 }
322 
323 static int mipid02_detect(struct mipid02_dev *bridge)
324 {
325 	u8 reg;
326 
327 	/*
328 	 * There is no version registers. Just try to read register
329 	 * MIPID02_CLK_LANE_WR_REG1.
330 	 */
331 	return mipid02_read_reg(bridge, MIPID02_CLK_LANE_WR_REG1, &reg);
332 }
333 
334 static u32 mipid02_get_link_freq_from_cid_pixel_rate(struct mipid02_dev *bridge,
335 						     struct v4l2_subdev *subdev)
336 {
337 	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
338 	struct v4l2_ctrl *ctrl;
339 	u32 pixel_clock;
340 	u32 bpp = bpp_from_code(bridge->fmt.code);
341 
342 	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
343 	if (!ctrl)
344 		return 0;
345 	pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
346 
347 	return pixel_clock * bpp / (2 * ep->bus.mipi_csi2.num_data_lanes);
348 }
349 
350 /*
351  * We need to know link frequency to setup clk_lane_reg1 timings. Link frequency
352  * will be computed using connected device V4L2_CID_PIXEL_RATE, bit per pixel
353  * and number of lanes.
354  */
355 static int mipid02_configure_from_rx_speed(struct mipid02_dev *bridge)
356 {
357 	struct i2c_client *client = bridge->i2c_client;
358 	struct v4l2_subdev *subdev = bridge->s_subdev;
359 	u32 link_freq;
360 
361 	link_freq = mipid02_get_link_freq_from_cid_pixel_rate(bridge, subdev);
362 	if (!link_freq) {
363 		dev_err(&client->dev, "Failed to detect link frequency");
364 		return -EINVAL;
365 	}
366 
367 	dev_dbg(&client->dev, "detect link_freq = %d Hz", link_freq);
368 	bridge->r.clk_lane_reg1 |= (2000000000 / link_freq) << 2;
369 
370 	return 0;
371 }
372 
373 static int mipid02_configure_clk_lane(struct mipid02_dev *bridge)
374 {
375 	struct i2c_client *client = bridge->i2c_client;
376 	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
377 	bool *polarities = ep->bus.mipi_csi2.lane_polarities;
378 
379 	/* midid02 doesn't support clock lane remapping */
380 	if (ep->bus.mipi_csi2.clock_lane != 0) {
381 		dev_err(&client->dev, "clk lane must be map to lane 0\n");
382 		return -EINVAL;
383 	}
384 	bridge->r.clk_lane_reg1 |= (polarities[0] << 1) | CLK_ENABLE;
385 
386 	return 0;
387 }
388 
389 static int mipid02_configure_data0_lane(struct mipid02_dev *bridge, int nb,
390 					bool are_lanes_swap, bool *polarities)
391 {
392 	bool are_pin_swap = are_lanes_swap ? polarities[2] : polarities[1];
393 
394 	if (nb == 1 && are_lanes_swap)
395 		return 0;
396 
397 	/*
398 	 * data lane 0 as pin swap polarity reversed compared to clock and
399 	 * data lane 1
400 	 */
401 	if (!are_pin_swap)
402 		bridge->r.data_lane0_reg1 = 1 << 1;
403 	bridge->r.data_lane0_reg1 |= DATA_ENABLE;
404 
405 	return 0;
406 }
407 
408 static int mipid02_configure_data1_lane(struct mipid02_dev *bridge, int nb,
409 					bool are_lanes_swap, bool *polarities)
410 {
411 	bool are_pin_swap = are_lanes_swap ? polarities[1] : polarities[2];
412 
413 	if (nb == 1 && !are_lanes_swap)
414 		return 0;
415 
416 	if (are_pin_swap)
417 		bridge->r.data_lane1_reg1 = 1 << 1;
418 	bridge->r.data_lane1_reg1 |= DATA_ENABLE;
419 
420 	return 0;
421 }
422 
423 static int mipid02_configure_from_rx(struct mipid02_dev *bridge)
424 {
425 	struct v4l2_fwnode_endpoint *ep = &bridge->rx;
426 	bool are_lanes_swap = ep->bus.mipi_csi2.data_lanes[0] == 2;
427 	bool *polarities = ep->bus.mipi_csi2.lane_polarities;
428 	int nb = ep->bus.mipi_csi2.num_data_lanes;
429 	int ret;
430 
431 	ret = mipid02_configure_clk_lane(bridge);
432 	if (ret)
433 		return ret;
434 
435 	ret = mipid02_configure_data0_lane(bridge, nb, are_lanes_swap,
436 					   polarities);
437 	if (ret)
438 		return ret;
439 
440 	ret = mipid02_configure_data1_lane(bridge, nb, are_lanes_swap,
441 					   polarities);
442 	if (ret)
443 		return ret;
444 
445 	bridge->r.mode_reg1 |= are_lanes_swap ? MODE_DATA_SWAP : 0;
446 	bridge->r.mode_reg1 |= (nb - 1) << 1;
447 
448 	return mipid02_configure_from_rx_speed(bridge);
449 }
450 
451 static int mipid02_configure_from_tx(struct mipid02_dev *bridge)
452 {
453 	struct v4l2_fwnode_endpoint *ep = &bridge->tx;
454 
455 	bridge->r.pix_width_ctrl = ep->bus.parallel.bus_width;
456 	bridge->r.pix_width_ctrl_emb = ep->bus.parallel.bus_width;
457 	if (ep->bus.parallel.flags & V4L2_MBUS_HSYNC_ACTIVE_HIGH)
458 		bridge->r.mode_reg2 |= MODE_HSYNC_ACTIVE_HIGH;
459 	if (ep->bus.parallel.flags & V4L2_MBUS_VSYNC_ACTIVE_HIGH)
460 		bridge->r.mode_reg2 |= MODE_VSYNC_ACTIVE_HIGH;
461 
462 	return 0;
463 }
464 
465 static int mipid02_configure_from_code(struct mipid02_dev *bridge)
466 {
467 	u8 data_type;
468 
469 	bridge->r.data_id_rreg = 0;
470 	data_type = data_type_from_code(bridge->fmt.code);
471 	if (!data_type)
472 		return -EINVAL;
473 	bridge->r.data_id_rreg = data_type;
474 
475 	return 0;
476 }
477 
478 static int mipid02_stream_disable(struct mipid02_dev *bridge)
479 {
480 	struct i2c_client *client = bridge->i2c_client;
481 	int ret;
482 
483 	/* Disable all lanes */
484 	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1, 0);
485 	if (ret)
486 		goto error;
487 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1, 0);
488 	if (ret)
489 		goto error;
490 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1, 0);
491 	if (ret)
492 		goto error;
493 error:
494 	if (ret)
495 		dev_err(&client->dev, "failed to stream off %d", ret);
496 
497 	return ret;
498 }
499 
500 static int mipid02_stream_enable(struct mipid02_dev *bridge)
501 {
502 	struct i2c_client *client = bridge->i2c_client;
503 	int ret = -EINVAL;
504 
505 	if (!bridge->s_subdev)
506 		goto error;
507 
508 	memset(&bridge->r, 0, sizeof(bridge->r));
509 	/* build registers content */
510 	ret = mipid02_configure_from_rx(bridge);
511 	if (ret)
512 		goto error;
513 	ret = mipid02_configure_from_tx(bridge);
514 	if (ret)
515 		goto error;
516 	ret = mipid02_configure_from_code(bridge);
517 	if (ret)
518 		goto error;
519 
520 	/* write mipi registers */
521 	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG1,
522 		bridge->r.clk_lane_reg1);
523 	if (ret)
524 		goto error;
525 	ret = mipid02_write_reg(bridge, MIPID02_CLK_LANE_REG3, CLK_MIPI_CSI);
526 	if (ret)
527 		goto error;
528 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG1,
529 		bridge->r.data_lane0_reg1);
530 	if (ret)
531 		goto error;
532 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE0_REG2,
533 		DATA_MIPI_CSI);
534 	if (ret)
535 		goto error;
536 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG1,
537 		bridge->r.data_lane1_reg1);
538 	if (ret)
539 		goto error;
540 	ret = mipid02_write_reg(bridge, MIPID02_DATA_LANE1_REG2,
541 		DATA_MIPI_CSI);
542 	if (ret)
543 		goto error;
544 	ret = mipid02_write_reg(bridge, MIPID02_MODE_REG1,
545 		MODE_NO_BYPASS | bridge->r.mode_reg1);
546 	if (ret)
547 		goto error;
548 	ret = mipid02_write_reg(bridge, MIPID02_MODE_REG2,
549 		bridge->r.mode_reg2);
550 	if (ret)
551 		goto error;
552 	ret = mipid02_write_reg(bridge, MIPID02_DATA_ID_RREG,
553 		bridge->r.data_id_rreg);
554 	if (ret)
555 		goto error;
556 	ret = mipid02_write_reg(bridge, MIPID02_DATA_SELECTION_CTRL,
557 		SELECTION_MANUAL_DATA | SELECTION_MANUAL_WIDTH);
558 	if (ret)
559 		goto error;
560 	ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL,
561 		bridge->r.pix_width_ctrl);
562 	if (ret)
563 		goto error;
564 	ret = mipid02_write_reg(bridge, MIPID02_PIX_WIDTH_CTRL_EMB,
565 		bridge->r.pix_width_ctrl_emb);
566 	if (ret)
567 		goto error;
568 
569 	return 0;
570 
571 error:
572 	dev_err(&client->dev, "failed to stream on %d", ret);
573 	mipid02_stream_disable(bridge);
574 
575 	return ret;
576 }
577 
578 static int mipid02_s_stream(struct v4l2_subdev *sd, int enable)
579 {
580 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
581 	struct i2c_client *client = bridge->i2c_client;
582 	int ret = 0;
583 
584 	dev_dbg(&client->dev, "%s : requested %d / current = %d", __func__,
585 		    enable, bridge->streaming);
586 	mutex_lock(&bridge->lock);
587 
588 	if (bridge->streaming == enable)
589 		goto out;
590 
591 	ret = enable ? mipid02_stream_enable(bridge) :
592 		       mipid02_stream_disable(bridge);
593 	if (!ret)
594 		bridge->streaming = enable;
595 
596 out:
597 	dev_dbg(&client->dev, "%s current now = %d / %d", __func__,
598 		    bridge->streaming, ret);
599 	mutex_unlock(&bridge->lock);
600 
601 	return ret;
602 }
603 
604 static int mipid02_enum_mbus_code(struct v4l2_subdev *sd,
605 				 struct v4l2_subdev_pad_config *cfg,
606 				 struct v4l2_subdev_mbus_code_enum *code)
607 {
608 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
609 	int ret = 0;
610 
611 	switch (code->pad) {
612 	case MIPID02_SINK_0:
613 		if (code->index >= ARRAY_SIZE(mipid02_supported_fmt_codes))
614 			ret = -EINVAL;
615 		else
616 			code->code = mipid02_supported_fmt_codes[code->index];
617 		break;
618 	case MIPID02_SOURCE:
619 		if (code->index == 0)
620 			code->code = serial_to_parallel_code(bridge->fmt.code);
621 		else
622 			ret = -EINVAL;
623 		break;
624 	default:
625 		ret = -EINVAL;
626 	}
627 
628 	return ret;
629 }
630 
631 static int mipid02_get_fmt(struct v4l2_subdev *sd,
632 			   struct v4l2_subdev_pad_config *cfg,
633 			   struct v4l2_subdev_format *format)
634 {
635 	struct v4l2_mbus_framefmt *mbus_fmt = &format->format;
636 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
637 	struct i2c_client *client = bridge->i2c_client;
638 	struct v4l2_mbus_framefmt *fmt;
639 
640 	dev_dbg(&client->dev, "%s probe %d", __func__, format->pad);
641 
642 	if (format->pad >= MIPID02_PAD_NB)
643 		return -EINVAL;
644 	/* second CSI-2 pad not yet supported */
645 	if (format->pad == MIPID02_SINK_1)
646 		return -EINVAL;
647 
648 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
649 		fmt = v4l2_subdev_get_try_format(&bridge->sd, cfg, format->pad);
650 	else
651 		fmt = &bridge->fmt;
652 
653 	mutex_lock(&bridge->lock);
654 
655 	*mbus_fmt = *fmt;
656 	/* code may need to be converted for source */
657 	if (format->pad == MIPID02_SOURCE)
658 		mbus_fmt->code = serial_to_parallel_code(mbus_fmt->code);
659 
660 	mutex_unlock(&bridge->lock);
661 
662 	return 0;
663 }
664 
665 static void mipid02_set_fmt_source(struct v4l2_subdev *sd,
666 				   struct v4l2_subdev_pad_config *cfg,
667 				   struct v4l2_subdev_format *format)
668 {
669 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
670 
671 	/* source pad mirror active sink pad */
672 	format->format = bridge->fmt;
673 	/* but code may need to be converted */
674 	format->format.code = serial_to_parallel_code(format->format.code);
675 
676 	/* only apply format for V4L2_SUBDEV_FORMAT_TRY case */
677 	if (format->which != V4L2_SUBDEV_FORMAT_TRY)
678 		return;
679 
680 	*v4l2_subdev_get_try_format(sd, cfg, format->pad) = format->format;
681 }
682 
683 static void mipid02_set_fmt_sink(struct v4l2_subdev *sd,
684 				 struct v4l2_subdev_pad_config *cfg,
685 				 struct v4l2_subdev_format *format)
686 {
687 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
688 	struct v4l2_mbus_framefmt *fmt;
689 
690 	format->format.code = get_fmt_code(format->format.code);
691 
692 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
693 		fmt = v4l2_subdev_get_try_format(sd, cfg, format->pad);
694 	else
695 		fmt = &bridge->fmt;
696 
697 	*fmt = format->format;
698 }
699 
700 static int mipid02_set_fmt(struct v4l2_subdev *sd,
701 			   struct v4l2_subdev_pad_config *cfg,
702 			   struct v4l2_subdev_format *format)
703 {
704 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
705 	struct i2c_client *client = bridge->i2c_client;
706 	int ret = 0;
707 
708 	dev_dbg(&client->dev, "%s for %d", __func__, format->pad);
709 
710 	if (format->pad >= MIPID02_PAD_NB)
711 		return -EINVAL;
712 	/* second CSI-2 pad not yet supported */
713 	if (format->pad == MIPID02_SINK_1)
714 		return -EINVAL;
715 
716 	mutex_lock(&bridge->lock);
717 
718 	if (bridge->streaming) {
719 		ret = -EBUSY;
720 		goto error;
721 	}
722 
723 	if (format->pad == MIPID02_SOURCE)
724 		mipid02_set_fmt_source(sd, cfg, format);
725 	else
726 		mipid02_set_fmt_sink(sd, cfg, format);
727 
728 error:
729 	mutex_unlock(&bridge->lock);
730 
731 	return ret;
732 }
733 
734 static const struct v4l2_subdev_video_ops mipid02_video_ops = {
735 	.s_stream = mipid02_s_stream,
736 };
737 
738 static const struct v4l2_subdev_pad_ops mipid02_pad_ops = {
739 	.enum_mbus_code = mipid02_enum_mbus_code,
740 	.get_fmt = mipid02_get_fmt,
741 	.set_fmt = mipid02_set_fmt,
742 };
743 
744 static const struct v4l2_subdev_ops mipid02_subdev_ops = {
745 	.video = &mipid02_video_ops,
746 	.pad = &mipid02_pad_ops,
747 };
748 
749 static const struct media_entity_operations mipid02_subdev_entity_ops = {
750 	.link_validate = v4l2_subdev_link_validate,
751 };
752 
753 static int mipid02_async_bound(struct v4l2_async_notifier *notifier,
754 			       struct v4l2_subdev *s_subdev,
755 			       struct v4l2_async_subdev *asd)
756 {
757 	struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd);
758 	struct i2c_client *client = bridge->i2c_client;
759 	int source_pad;
760 	int ret;
761 
762 	dev_dbg(&client->dev, "sensor_async_bound call %p", s_subdev);
763 
764 	source_pad = media_entity_get_fwnode_pad(&s_subdev->entity,
765 						 s_subdev->fwnode,
766 						 MEDIA_PAD_FL_SOURCE);
767 	if (source_pad < 0) {
768 		dev_err(&client->dev, "Couldn't find output pad for subdev %s\n",
769 			s_subdev->name);
770 		return source_pad;
771 	}
772 
773 	ret = media_create_pad_link(&s_subdev->entity, source_pad,
774 				    &bridge->sd.entity, 0,
775 				    MEDIA_LNK_FL_ENABLED |
776 				    MEDIA_LNK_FL_IMMUTABLE);
777 	if (ret) {
778 		dev_err(&client->dev, "Couldn't create media link %d", ret);
779 		return ret;
780 	}
781 
782 	bridge->s_subdev = s_subdev;
783 
784 	return 0;
785 }
786 
787 static void mipid02_async_unbind(struct v4l2_async_notifier *notifier,
788 				 struct v4l2_subdev *s_subdev,
789 				 struct v4l2_async_subdev *asd)
790 {
791 	struct mipid02_dev *bridge = to_mipid02_dev(notifier->sd);
792 
793 	bridge->s_subdev = NULL;
794 }
795 
796 static const struct v4l2_async_notifier_operations mipid02_notifier_ops = {
797 	.bound		= mipid02_async_bound,
798 	.unbind		= mipid02_async_unbind,
799 };
800 
801 static int mipid02_parse_rx_ep(struct mipid02_dev *bridge)
802 {
803 	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_CSI2_DPHY };
804 	struct i2c_client *client = bridge->i2c_client;
805 	struct device_node *ep_node;
806 	int ret;
807 
808 	/* parse rx (endpoint 0) */
809 	ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node,
810 						0, 0);
811 	if (!ep_node) {
812 		dev_err(&client->dev, "unable to find port0 ep");
813 		ret = -EINVAL;
814 		goto error;
815 	}
816 
817 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
818 	if (ret) {
819 		dev_err(&client->dev, "Could not parse v4l2 endpoint %d\n",
820 			ret);
821 		goto error_of_node_put;
822 	}
823 
824 	/* do some sanity checks */
825 	if (ep.bus.mipi_csi2.num_data_lanes > 2) {
826 		dev_err(&client->dev, "max supported data lanes is 2 / got %d",
827 			ep.bus.mipi_csi2.num_data_lanes);
828 		ret = -EINVAL;
829 		goto error_of_node_put;
830 	}
831 
832 	/* register it for later use */
833 	bridge->rx = ep;
834 
835 	/* register async notifier so we get noticed when sensor is connected */
836 	bridge->asd.match.fwnode =
837 		fwnode_graph_get_remote_port_parent(of_fwnode_handle(ep_node));
838 	bridge->asd.match_type = V4L2_ASYNC_MATCH_FWNODE;
839 	of_node_put(ep_node);
840 
841 	v4l2_async_notifier_init(&bridge->notifier);
842 	ret = v4l2_async_notifier_add_subdev(&bridge->notifier, &bridge->asd);
843 	if (ret) {
844 		dev_err(&client->dev, "fail to register asd to notifier %d",
845 			ret);
846 		fwnode_handle_put(bridge->asd.match.fwnode);
847 		return ret;
848 	}
849 	bridge->notifier.ops = &mipid02_notifier_ops;
850 
851 	ret = v4l2_async_subdev_notifier_register(&bridge->sd,
852 						  &bridge->notifier);
853 	if (ret)
854 		v4l2_async_notifier_cleanup(&bridge->notifier);
855 
856 	return ret;
857 
858 error_of_node_put:
859 	of_node_put(ep_node);
860 error:
861 
862 	return ret;
863 }
864 
865 static int mipid02_parse_tx_ep(struct mipid02_dev *bridge)
866 {
867 	struct v4l2_fwnode_endpoint ep = { .bus_type = V4L2_MBUS_PARALLEL };
868 	struct i2c_client *client = bridge->i2c_client;
869 	struct device_node *ep_node;
870 	int ret;
871 
872 	/* parse tx (endpoint 2) */
873 	ep_node = of_graph_get_endpoint_by_regs(bridge->i2c_client->dev.of_node,
874 						2, 0);
875 	if (!ep_node) {
876 		dev_err(&client->dev, "unable to find port1 ep");
877 		ret = -EINVAL;
878 		goto error;
879 	}
880 
881 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep_node), &ep);
882 	if (ret) {
883 		dev_err(&client->dev, "Could not parse v4l2 endpoint\n");
884 		goto error_of_node_put;
885 	}
886 
887 	of_node_put(ep_node);
888 	bridge->tx = ep;
889 
890 	return 0;
891 
892 error_of_node_put:
893 	of_node_put(ep_node);
894 error:
895 
896 	return -EINVAL;
897 }
898 
899 static int mipid02_probe(struct i2c_client *client)
900 {
901 	struct device *dev = &client->dev;
902 	struct mipid02_dev *bridge;
903 	u32 clk_freq;
904 	int ret;
905 
906 	bridge = devm_kzalloc(dev, sizeof(*bridge), GFP_KERNEL);
907 	if (!bridge)
908 		return -ENOMEM;
909 
910 	init_format(&bridge->fmt);
911 
912 	bridge->i2c_client = client;
913 	v4l2_i2c_subdev_init(&bridge->sd, client, &mipid02_subdev_ops);
914 
915 	/* got and check clock */
916 	bridge->xclk = devm_clk_get(dev, "xclk");
917 	if (IS_ERR(bridge->xclk)) {
918 		dev_err(dev, "failed to get xclk\n");
919 		return PTR_ERR(bridge->xclk);
920 	}
921 
922 	clk_freq = clk_get_rate(bridge->xclk);
923 	if (clk_freq < 6000000 || clk_freq > 27000000) {
924 		dev_err(dev, "xclk freq must be in 6-27 Mhz range. got %d Hz\n",
925 			clk_freq);
926 		return -EINVAL;
927 	}
928 
929 	bridge->reset_gpio = devm_gpiod_get_optional(dev, "reset",
930 						     GPIOD_OUT_HIGH);
931 
932 	ret = mipid02_get_regulators(bridge);
933 	if (ret) {
934 		dev_err(dev, "failed to get regulators %d", ret);
935 		return ret;
936 	}
937 
938 	mutex_init(&bridge->lock);
939 	bridge->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
940 	bridge->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
941 	bridge->sd.entity.ops = &mipid02_subdev_entity_ops;
942 	bridge->pad[0].flags = MEDIA_PAD_FL_SINK;
943 	bridge->pad[1].flags = MEDIA_PAD_FL_SINK;
944 	bridge->pad[2].flags = MEDIA_PAD_FL_SOURCE;
945 	ret = media_entity_pads_init(&bridge->sd.entity, MIPID02_PAD_NB,
946 				     bridge->pad);
947 	if (ret) {
948 		dev_err(&client->dev, "pads init failed %d", ret);
949 		goto mutex_cleanup;
950 	}
951 
952 	/* enable clock, power and reset device if available */
953 	ret = mipid02_set_power_on(bridge);
954 	if (ret)
955 		goto entity_cleanup;
956 
957 	ret = mipid02_detect(bridge);
958 	if (ret) {
959 		dev_err(&client->dev, "failed to detect mipid02 %d", ret);
960 		goto power_off;
961 	}
962 
963 	ret = mipid02_parse_tx_ep(bridge);
964 	if (ret) {
965 		dev_err(&client->dev, "failed to parse tx %d", ret);
966 		goto power_off;
967 	}
968 
969 	ret = mipid02_parse_rx_ep(bridge);
970 	if (ret) {
971 		dev_err(&client->dev, "failed to parse rx %d", ret);
972 		goto power_off;
973 	}
974 
975 	ret = v4l2_async_register_subdev(&bridge->sd);
976 	if (ret < 0) {
977 		dev_err(&client->dev, "v4l2_async_register_subdev failed %d",
978 			    ret);
979 		goto unregister_notifier;
980 	}
981 
982 	dev_info(&client->dev, "mipid02 device probe successfully");
983 
984 	return 0;
985 
986 unregister_notifier:
987 	v4l2_async_notifier_unregister(&bridge->notifier);
988 	v4l2_async_notifier_cleanup(&bridge->notifier);
989 power_off:
990 	mipid02_set_power_off(bridge);
991 entity_cleanup:
992 	media_entity_cleanup(&bridge->sd.entity);
993 mutex_cleanup:
994 	mutex_destroy(&bridge->lock);
995 
996 	return ret;
997 }
998 
999 static int mipid02_remove(struct i2c_client *client)
1000 {
1001 	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1002 	struct mipid02_dev *bridge = to_mipid02_dev(sd);
1003 
1004 	v4l2_async_notifier_unregister(&bridge->notifier);
1005 	v4l2_async_notifier_cleanup(&bridge->notifier);
1006 	v4l2_async_unregister_subdev(&bridge->sd);
1007 	mipid02_set_power_off(bridge);
1008 	media_entity_cleanup(&bridge->sd.entity);
1009 	mutex_destroy(&bridge->lock);
1010 
1011 	return 0;
1012 }
1013 
1014 static const struct of_device_id mipid02_dt_ids[] = {
1015 	{ .compatible = "st,st-mipid02" },
1016 	{ /* sentinel */ }
1017 };
1018 MODULE_DEVICE_TABLE(of, mipid02_dt_ids);
1019 
1020 static struct i2c_driver mipid02_i2c_driver = {
1021 	.driver = {
1022 		.name  = "st-mipid02",
1023 		.of_match_table = mipid02_dt_ids,
1024 	},
1025 	.probe_new = mipid02_probe,
1026 	.remove = mipid02_remove,
1027 };
1028 
1029 module_i2c_driver(mipid02_i2c_driver);
1030 
1031 MODULE_AUTHOR("Mickael Guene <mickael.guene@st.com>");
1032 MODULE_DESCRIPTION("STMicroelectronics MIPID02 CSI-2 bridge driver");
1033 MODULE_LICENSE("GPL v2");
1034