1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * Copyright 2020-2022 Bootlin
4  * Author: Paul Kocialkowski <paul.kocialkowski@bootlin.com>
5  */
6 
7 #include <linux/clk.h>
8 #include <linux/module.h>
9 #include <linux/of.h>
10 #include <linux/of_device.h>
11 #include <linux/phy/phy.h>
12 #include <linux/platform_device.h>
13 #include <linux/pm_runtime.h>
14 #include <linux/regmap.h>
15 #include <linux/reset.h>
16 #include <media/mipi-csi2.h>
17 #include <media/v4l2-ctrls.h>
18 #include <media/v4l2-device.h>
19 #include <media/v4l2-fwnode.h>
20 
21 #include "sun6i_mipi_csi2.h"
22 #include "sun6i_mipi_csi2_reg.h"
23 
24 /* Format */
25 
26 static const struct sun6i_mipi_csi2_format sun6i_mipi_csi2_formats[] = {
27 	{
28 		.mbus_code	= MEDIA_BUS_FMT_SBGGR8_1X8,
29 		.data_type	= MIPI_CSI2_DT_RAW8,
30 		.bpp		= 8,
31 	},
32 	{
33 		.mbus_code	= MEDIA_BUS_FMT_SGBRG8_1X8,
34 		.data_type	= MIPI_CSI2_DT_RAW8,
35 		.bpp		= 8,
36 	},
37 	{
38 		.mbus_code	= MEDIA_BUS_FMT_SGRBG8_1X8,
39 		.data_type	= MIPI_CSI2_DT_RAW8,
40 		.bpp		= 8,
41 	},
42 	{
43 		.mbus_code	= MEDIA_BUS_FMT_SRGGB8_1X8,
44 		.data_type	= MIPI_CSI2_DT_RAW8,
45 		.bpp		= 8,
46 	},
47 	{
48 		.mbus_code	= MEDIA_BUS_FMT_SBGGR10_1X10,
49 		.data_type	= MIPI_CSI2_DT_RAW10,
50 		.bpp		= 10,
51 	},
52 	{
53 		.mbus_code	= MEDIA_BUS_FMT_SGBRG10_1X10,
54 		.data_type	= MIPI_CSI2_DT_RAW10,
55 		.bpp		= 10,
56 	},
57 	{
58 		.mbus_code	= MEDIA_BUS_FMT_SGRBG10_1X10,
59 		.data_type	= MIPI_CSI2_DT_RAW10,
60 		.bpp		= 10,
61 	},
62 	{
63 		.mbus_code	= MEDIA_BUS_FMT_SRGGB10_1X10,
64 		.data_type	= MIPI_CSI2_DT_RAW10,
65 		.bpp		= 10,
66 	},
67 };
68 
69 static const struct sun6i_mipi_csi2_format *
70 sun6i_mipi_csi2_format_find(u32 mbus_code)
71 {
72 	unsigned int i;
73 
74 	for (i = 0; i < ARRAY_SIZE(sun6i_mipi_csi2_formats); i++)
75 		if (sun6i_mipi_csi2_formats[i].mbus_code == mbus_code)
76 			return &sun6i_mipi_csi2_formats[i];
77 
78 	return NULL;
79 }
80 
81 /* Controller */
82 
83 static void sun6i_mipi_csi2_enable(struct sun6i_mipi_csi2_device *csi2_dev)
84 {
85 	struct regmap *regmap = csi2_dev->regmap;
86 
87 	regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
88 			   SUN6I_MIPI_CSI2_CTL_EN, SUN6I_MIPI_CSI2_CTL_EN);
89 }
90 
91 static void sun6i_mipi_csi2_disable(struct sun6i_mipi_csi2_device *csi2_dev)
92 {
93 	struct regmap *regmap = csi2_dev->regmap;
94 
95 	regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
96 			   SUN6I_MIPI_CSI2_CTL_EN, 0);
97 }
98 
99 static void sun6i_mipi_csi2_configure(struct sun6i_mipi_csi2_device *csi2_dev)
100 {
101 	struct regmap *regmap = csi2_dev->regmap;
102 	unsigned int lanes_count =
103 		csi2_dev->bridge.endpoint.bus.mipi_csi2.num_data_lanes;
104 	struct v4l2_mbus_framefmt *mbus_format = &csi2_dev->bridge.mbus_format;
105 	const struct sun6i_mipi_csi2_format *format;
106 	struct device *dev = csi2_dev->dev;
107 	u32 version = 0;
108 
109 	format = sun6i_mipi_csi2_format_find(mbus_format->code);
110 	if (WARN_ON(!format))
111 		return;
112 
113 	/*
114 	 * The enable flow in the Allwinner BSP is a bit different: the enable
115 	 * and reset bits are set together before starting the CSI controller.
116 	 *
117 	 * In mainline we enable the CSI controller first (due to subdev logic).
118 	 * One reliable way to make this work is to deassert reset, configure
119 	 * registers and enable the controller when everything's ready.
120 	 *
121 	 * However, setting the version enable bit and removing it afterwards
122 	 * appears necessary for capture to work reliably, while replacing it
123 	 * with a delay doesn't do the trick.
124 	 */
125 	regmap_write(regmap, SUN6I_MIPI_CSI2_CTL_REG,
126 		     SUN6I_MIPI_CSI2_CTL_RESET_N |
127 		     SUN6I_MIPI_CSI2_CTL_VERSION_EN |
128 		     SUN6I_MIPI_CSI2_CTL_UNPK_EN);
129 
130 	regmap_read(regmap, SUN6I_MIPI_CSI2_VERSION_REG, &version);
131 
132 	regmap_update_bits(regmap, SUN6I_MIPI_CSI2_CTL_REG,
133 			   SUN6I_MIPI_CSI2_CTL_VERSION_EN, 0);
134 
135 	dev_dbg(dev, "A31 MIPI CSI-2 version: %04x\n", version);
136 
137 	regmap_write(regmap, SUN6I_MIPI_CSI2_CFG_REG,
138 		     SUN6I_MIPI_CSI2_CFG_CHANNEL_MODE(1) |
139 		     SUN6I_MIPI_CSI2_CFG_LANE_COUNT(lanes_count));
140 
141 	/*
142 	 * Only a single virtual channel (index 0) is currently supported.
143 	 * While the registers do mention multiple physical channels being
144 	 * available (which can be configured to match a specific virtual
145 	 * channel or data type), it's unclear whether channels > 0 are actually
146 	 * connected and available and the reference source code only makes use
147 	 * of channel 0.
148 	 *
149 	 * Using extra channels would also require matching channels to be
150 	 * available on the CSI (and ISP) side, which is also unsure although
151 	 * some CSI implementations are said to support multiple channels for
152 	 * BT656 time-sharing.
153 	 *
154 	 * We still configure virtual channel numbers to ensure that virtual
155 	 * channel 0 only goes to channel 0.
156 	 */
157 
158 	regmap_write(regmap, SUN6I_MIPI_CSI2_VCDT_RX_REG,
159 		     SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(3, 3) |
160 		     SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(2, 2) |
161 		     SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(1, 1) |
162 		     SUN6I_MIPI_CSI2_VCDT_RX_CH_VC(0, 0) |
163 		     SUN6I_MIPI_CSI2_VCDT_RX_CH_DT(0, format->data_type));
164 
165 	regmap_write(regmap, SUN6I_MIPI_CSI2_CH_INT_PD_REG,
166 		     SUN6I_MIPI_CSI2_CH_INT_PD_CLEAR);
167 }
168 
169 /* V4L2 Subdev */
170 
171 static int sun6i_mipi_csi2_s_stream(struct v4l2_subdev *subdev, int on)
172 {
173 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
174 	struct v4l2_subdev *source_subdev = csi2_dev->bridge.source_subdev;
175 	union phy_configure_opts dphy_opts = { 0 };
176 	struct phy_configure_opts_mipi_dphy *dphy_cfg = &dphy_opts.mipi_dphy;
177 	struct v4l2_mbus_framefmt *mbus_format = &csi2_dev->bridge.mbus_format;
178 	const struct sun6i_mipi_csi2_format *format;
179 	struct phy *dphy = csi2_dev->dphy;
180 	struct device *dev = csi2_dev->dev;
181 	struct v4l2_ctrl *ctrl;
182 	unsigned int lanes_count =
183 		csi2_dev->bridge.endpoint.bus.mipi_csi2.num_data_lanes;
184 	unsigned long pixel_rate;
185 	int ret;
186 
187 	if (!source_subdev)
188 		return -ENODEV;
189 
190 	if (!on) {
191 		ret = v4l2_subdev_call(source_subdev, video, s_stream, 0);
192 		goto disable;
193 	}
194 
195 	/* Runtime PM */
196 
197 	ret = pm_runtime_resume_and_get(dev);
198 	if (ret < 0)
199 		return ret;
200 
201 	/* Sensor Pixel Rate */
202 
203 	ctrl = v4l2_ctrl_find(source_subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
204 	if (!ctrl) {
205 		dev_err(dev, "missing sensor pixel rate\n");
206 		ret = -ENODEV;
207 		goto error_pm;
208 	}
209 
210 	pixel_rate = (unsigned long)v4l2_ctrl_g_ctrl_int64(ctrl);
211 	if (!pixel_rate) {
212 		dev_err(dev, "missing (zero) sensor pixel rate\n");
213 		ret = -ENODEV;
214 		goto error_pm;
215 	}
216 
217 	/* D-PHY */
218 
219 	if (!lanes_count) {
220 		dev_err(dev, "missing (zero) MIPI CSI-2 lanes count\n");
221 		ret = -ENODEV;
222 		goto error_pm;
223 	}
224 
225 	format = sun6i_mipi_csi2_format_find(mbus_format->code);
226 	if (WARN_ON(!format)) {
227 		ret = -ENODEV;
228 		goto error_pm;
229 	}
230 
231 	phy_mipi_dphy_get_default_config(pixel_rate, format->bpp, lanes_count,
232 					 dphy_cfg);
233 
234 	/*
235 	 * Note that our hardware is using DDR, which is not taken in account by
236 	 * phy_mipi_dphy_get_default_config when calculating hs_clk_rate from
237 	 * the pixel rate, lanes count and bpp.
238 	 *
239 	 * The resulting clock rate is basically the symbol rate over the whole
240 	 * link. The actual clock rate is calculated with division by two since
241 	 * DDR samples both on rising and falling edges.
242 	 */
243 
244 	dev_dbg(dev, "A31 MIPI CSI-2 config:\n");
245 	dev_dbg(dev, "%ld pixels/s, %u bits/pixel, %u lanes, %lu Hz clock\n",
246 		pixel_rate, format->bpp, lanes_count,
247 		dphy_cfg->hs_clk_rate / 2);
248 
249 	ret = phy_reset(dphy);
250 	if (ret) {
251 		dev_err(dev, "failed to reset MIPI D-PHY\n");
252 		goto error_pm;
253 	}
254 
255 	ret = phy_configure(dphy, &dphy_opts);
256 	if (ret) {
257 		dev_err(dev, "failed to configure MIPI D-PHY\n");
258 		goto error_pm;
259 	}
260 
261 	/* Controller */
262 
263 	sun6i_mipi_csi2_configure(csi2_dev);
264 	sun6i_mipi_csi2_enable(csi2_dev);
265 
266 	/* D-PHY */
267 
268 	ret = phy_power_on(dphy);
269 	if (ret) {
270 		dev_err(dev, "failed to power on MIPI D-PHY\n");
271 		goto error_pm;
272 	}
273 
274 	/* Source */
275 
276 	ret = v4l2_subdev_call(source_subdev, video, s_stream, 1);
277 	if (ret && ret != -ENOIOCTLCMD)
278 		goto disable;
279 
280 	return 0;
281 
282 disable:
283 	if (!on)
284 		ret = 0;
285 	phy_power_off(dphy);
286 	sun6i_mipi_csi2_disable(csi2_dev);
287 
288 error_pm:
289 	pm_runtime_put(dev);
290 
291 	return ret;
292 }
293 
294 static const struct v4l2_subdev_video_ops sun6i_mipi_csi2_video_ops = {
295 	.s_stream	= sun6i_mipi_csi2_s_stream,
296 };
297 
298 static void
299 sun6i_mipi_csi2_mbus_format_prepare(struct v4l2_mbus_framefmt *mbus_format)
300 {
301 	if (!sun6i_mipi_csi2_format_find(mbus_format->code))
302 		mbus_format->code = sun6i_mipi_csi2_formats[0].mbus_code;
303 
304 	mbus_format->field = V4L2_FIELD_NONE;
305 	mbus_format->colorspace = V4L2_COLORSPACE_RAW;
306 	mbus_format->quantization = V4L2_QUANTIZATION_DEFAULT;
307 	mbus_format->xfer_func = V4L2_XFER_FUNC_DEFAULT;
308 }
309 
310 static int sun6i_mipi_csi2_init_cfg(struct v4l2_subdev *subdev,
311 				    struct v4l2_subdev_state *state)
312 {
313 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
314 	unsigned int pad = SUN6I_MIPI_CSI2_PAD_SINK;
315 	struct v4l2_mbus_framefmt *mbus_format =
316 		v4l2_subdev_get_try_format(subdev, state, pad);
317 	struct mutex *lock = &csi2_dev->bridge.lock;
318 
319 	mutex_lock(lock);
320 
321 	mbus_format->code = sun6i_mipi_csi2_formats[0].mbus_code;
322 	mbus_format->width = 640;
323 	mbus_format->height = 480;
324 
325 	sun6i_mipi_csi2_mbus_format_prepare(mbus_format);
326 
327 	mutex_unlock(lock);
328 
329 	return 0;
330 }
331 
332 static int
333 sun6i_mipi_csi2_enum_mbus_code(struct v4l2_subdev *subdev,
334 			       struct v4l2_subdev_state *state,
335 			       struct v4l2_subdev_mbus_code_enum *code_enum)
336 {
337 	if (code_enum->index >= ARRAY_SIZE(sun6i_mipi_csi2_formats))
338 		return -EINVAL;
339 
340 	code_enum->code = sun6i_mipi_csi2_formats[code_enum->index].mbus_code;
341 
342 	return 0;
343 }
344 
345 static int sun6i_mipi_csi2_get_fmt(struct v4l2_subdev *subdev,
346 				   struct v4l2_subdev_state *state,
347 				   struct v4l2_subdev_format *format)
348 {
349 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
350 	struct v4l2_mbus_framefmt *mbus_format = &format->format;
351 	struct mutex *lock = &csi2_dev->bridge.lock;
352 
353 	mutex_lock(lock);
354 
355 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
356 		*mbus_format = *v4l2_subdev_get_try_format(subdev, state,
357 							   format->pad);
358 	else
359 		*mbus_format = csi2_dev->bridge.mbus_format;
360 
361 	mutex_unlock(lock);
362 
363 	return 0;
364 }
365 
366 static int sun6i_mipi_csi2_set_fmt(struct v4l2_subdev *subdev,
367 				   struct v4l2_subdev_state *state,
368 				   struct v4l2_subdev_format *format)
369 {
370 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
371 	struct v4l2_mbus_framefmt *mbus_format = &format->format;
372 	struct mutex *lock = &csi2_dev->bridge.lock;
373 
374 	mutex_lock(lock);
375 
376 	sun6i_mipi_csi2_mbus_format_prepare(mbus_format);
377 
378 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
379 		*v4l2_subdev_get_try_format(subdev, state, format->pad) =
380 			*mbus_format;
381 	else
382 		csi2_dev->bridge.mbus_format = *mbus_format;
383 
384 	mutex_unlock(lock);
385 
386 	return 0;
387 }
388 
389 static const struct v4l2_subdev_pad_ops sun6i_mipi_csi2_pad_ops = {
390 	.init_cfg	= sun6i_mipi_csi2_init_cfg,
391 	.enum_mbus_code	= sun6i_mipi_csi2_enum_mbus_code,
392 	.get_fmt	= sun6i_mipi_csi2_get_fmt,
393 	.set_fmt	= sun6i_mipi_csi2_set_fmt,
394 };
395 
396 static const struct v4l2_subdev_ops sun6i_mipi_csi2_subdev_ops = {
397 	.video	= &sun6i_mipi_csi2_video_ops,
398 	.pad	= &sun6i_mipi_csi2_pad_ops,
399 };
400 
401 /* Media Entity */
402 
403 static const struct media_entity_operations sun6i_mipi_csi2_entity_ops = {
404 	.link_validate	= v4l2_subdev_link_validate,
405 };
406 
407 /* V4L2 Async */
408 
409 static int
410 sun6i_mipi_csi2_notifier_bound(struct v4l2_async_notifier *notifier,
411 			       struct v4l2_subdev *remote_subdev,
412 			       struct v4l2_async_subdev *async_subdev)
413 {
414 	struct v4l2_subdev *subdev = notifier->sd;
415 	struct sun6i_mipi_csi2_device *csi2_dev =
416 		container_of(notifier, struct sun6i_mipi_csi2_device,
417 			     bridge.notifier);
418 	struct media_entity *sink_entity = &subdev->entity;
419 	struct media_entity *source_entity = &remote_subdev->entity;
420 	struct device *dev = csi2_dev->dev;
421 	int sink_pad_index = 0;
422 	int source_pad_index;
423 	int ret;
424 
425 	ret = media_entity_get_fwnode_pad(source_entity, remote_subdev->fwnode,
426 					  MEDIA_PAD_FL_SOURCE);
427 	if (ret < 0) {
428 		dev_err(dev, "missing source pad in external entity %s\n",
429 			source_entity->name);
430 		return -EINVAL;
431 	}
432 
433 	source_pad_index = ret;
434 
435 	dev_dbg(dev, "creating %s:%u -> %s:%u link\n", source_entity->name,
436 		source_pad_index, sink_entity->name, sink_pad_index);
437 
438 	ret = media_create_pad_link(source_entity, source_pad_index,
439 				    sink_entity, sink_pad_index,
440 				    MEDIA_LNK_FL_ENABLED |
441 				    MEDIA_LNK_FL_IMMUTABLE);
442 	if (ret) {
443 		dev_err(dev, "failed to create %s:%u -> %s:%u link\n",
444 			source_entity->name, source_pad_index,
445 			sink_entity->name, sink_pad_index);
446 		return ret;
447 	}
448 
449 	csi2_dev->bridge.source_subdev = remote_subdev;
450 
451 	return 0;
452 }
453 
454 static const struct v4l2_async_notifier_operations
455 sun6i_mipi_csi2_notifier_ops = {
456 	.bound	= sun6i_mipi_csi2_notifier_bound,
457 };
458 
459 /* Bridge */
460 
461 static int
462 sun6i_mipi_csi2_bridge_source_setup(struct sun6i_mipi_csi2_device *csi2_dev)
463 {
464 	struct v4l2_async_notifier *notifier = &csi2_dev->bridge.notifier;
465 	struct v4l2_fwnode_endpoint *endpoint = &csi2_dev->bridge.endpoint;
466 	struct v4l2_async_subdev *subdev_async;
467 	struct fwnode_handle *handle;
468 	struct device *dev = csi2_dev->dev;
469 	int ret;
470 
471 	handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
472 						 FWNODE_GRAPH_ENDPOINT_NEXT);
473 	if (!handle)
474 		return -ENODEV;
475 
476 	endpoint->bus_type = V4L2_MBUS_CSI2_DPHY;
477 
478 	ret = v4l2_fwnode_endpoint_parse(handle, endpoint);
479 	if (ret)
480 		goto complete;
481 
482 	subdev_async =
483 		v4l2_async_nf_add_fwnode_remote(notifier, handle,
484 						struct v4l2_async_subdev);
485 	if (IS_ERR(subdev_async))
486 		ret = PTR_ERR(subdev_async);
487 
488 complete:
489 	fwnode_handle_put(handle);
490 
491 	return ret;
492 }
493 
494 static int sun6i_mipi_csi2_bridge_setup(struct sun6i_mipi_csi2_device *csi2_dev)
495 {
496 	struct sun6i_mipi_csi2_bridge *bridge = &csi2_dev->bridge;
497 	struct v4l2_subdev *subdev = &bridge->subdev;
498 	struct v4l2_async_notifier *notifier = &bridge->notifier;
499 	struct media_pad *pads = bridge->pads;
500 	struct device *dev = csi2_dev->dev;
501 	int ret;
502 
503 	mutex_init(&bridge->lock);
504 
505 	/* V4L2 Subdev */
506 
507 	v4l2_subdev_init(subdev, &sun6i_mipi_csi2_subdev_ops);
508 	strscpy(subdev->name, SUN6I_MIPI_CSI2_NAME, sizeof(subdev->name));
509 	subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
510 	subdev->owner = THIS_MODULE;
511 	subdev->dev = dev;
512 
513 	v4l2_set_subdevdata(subdev, csi2_dev);
514 
515 	/* Media Entity */
516 
517 	subdev->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
518 	subdev->entity.ops = &sun6i_mipi_csi2_entity_ops;
519 
520 	/* Media Pads */
521 
522 	pads[SUN6I_MIPI_CSI2_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
523 	pads[SUN6I_MIPI_CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
524 
525 	ret = media_entity_pads_init(&subdev->entity, SUN6I_MIPI_CSI2_PAD_COUNT,
526 				     pads);
527 	if (ret)
528 		return ret;
529 
530 	/* V4L2 Async */
531 
532 	v4l2_async_nf_init(notifier);
533 	notifier->ops = &sun6i_mipi_csi2_notifier_ops;
534 
535 	ret = sun6i_mipi_csi2_bridge_source_setup(csi2_dev);
536 	if (ret)
537 		goto error_v4l2_notifier_cleanup;
538 
539 	ret = v4l2_async_subdev_nf_register(subdev, notifier);
540 	if (ret < 0)
541 		goto error_v4l2_notifier_cleanup;
542 
543 	/* V4L2 Subdev */
544 
545 	ret = v4l2_async_register_subdev(subdev);
546 	if (ret < 0)
547 		goto error_v4l2_notifier_unregister;
548 
549 	return 0;
550 
551 error_v4l2_notifier_unregister:
552 	v4l2_async_nf_unregister(notifier);
553 
554 error_v4l2_notifier_cleanup:
555 	v4l2_async_nf_cleanup(notifier);
556 
557 	media_entity_cleanup(&subdev->entity);
558 
559 	return ret;
560 }
561 
562 static void
563 sun6i_mipi_csi2_bridge_cleanup(struct sun6i_mipi_csi2_device *csi2_dev)
564 {
565 	struct v4l2_subdev *subdev = &csi2_dev->bridge.subdev;
566 	struct v4l2_async_notifier *notifier = &csi2_dev->bridge.notifier;
567 
568 	v4l2_async_unregister_subdev(subdev);
569 	v4l2_async_nf_unregister(notifier);
570 	v4l2_async_nf_cleanup(notifier);
571 	media_entity_cleanup(&subdev->entity);
572 }
573 
574 /* Platform */
575 
576 static int sun6i_mipi_csi2_suspend(struct device *dev)
577 {
578 	struct sun6i_mipi_csi2_device *csi2_dev = dev_get_drvdata(dev);
579 
580 	clk_disable_unprepare(csi2_dev->clock_mod);
581 	reset_control_assert(csi2_dev->reset);
582 
583 	return 0;
584 }
585 
586 static int sun6i_mipi_csi2_resume(struct device *dev)
587 {
588 	struct sun6i_mipi_csi2_device *csi2_dev = dev_get_drvdata(dev);
589 	int ret;
590 
591 	ret = reset_control_deassert(csi2_dev->reset);
592 	if (ret) {
593 		dev_err(dev, "failed to deassert reset\n");
594 		return ret;
595 	}
596 
597 	ret = clk_prepare_enable(csi2_dev->clock_mod);
598 	if (ret) {
599 		dev_err(dev, "failed to enable module clock\n");
600 		goto error_reset;
601 	}
602 
603 	return 0;
604 
605 error_reset:
606 	reset_control_assert(csi2_dev->reset);
607 
608 	return ret;
609 }
610 
611 static const struct dev_pm_ops sun6i_mipi_csi2_pm_ops = {
612 	.runtime_suspend	= sun6i_mipi_csi2_suspend,
613 	.runtime_resume		= sun6i_mipi_csi2_resume,
614 };
615 
616 static const struct regmap_config sun6i_mipi_csi2_regmap_config = {
617 	.reg_bits       = 32,
618 	.reg_stride     = 4,
619 	.val_bits       = 32,
620 	.max_register	= 0x400,
621 };
622 
623 static int
624 sun6i_mipi_csi2_resources_setup(struct sun6i_mipi_csi2_device *csi2_dev,
625 				struct platform_device *platform_dev)
626 {
627 	struct device *dev = csi2_dev->dev;
628 	void __iomem *io_base;
629 	int ret;
630 
631 	/* Registers */
632 
633 	io_base = devm_platform_ioremap_resource(platform_dev, 0);
634 	if (IS_ERR(io_base))
635 		return PTR_ERR(io_base);
636 
637 	csi2_dev->regmap =
638 		devm_regmap_init_mmio_clk(dev, "bus", io_base,
639 					  &sun6i_mipi_csi2_regmap_config);
640 	if (IS_ERR(csi2_dev->regmap)) {
641 		dev_err(dev, "failed to init register map\n");
642 		return PTR_ERR(csi2_dev->regmap);
643 	}
644 
645 	/* Clock */
646 
647 	csi2_dev->clock_mod = devm_clk_get(dev, "mod");
648 	if (IS_ERR(csi2_dev->clock_mod)) {
649 		dev_err(dev, "failed to acquire mod clock\n");
650 		return PTR_ERR(csi2_dev->clock_mod);
651 	}
652 
653 	ret = clk_set_rate_exclusive(csi2_dev->clock_mod, 297000000);
654 	if (ret) {
655 		dev_err(dev, "failed to set mod clock rate\n");
656 		return ret;
657 	}
658 
659 	/* Reset */
660 
661 	csi2_dev->reset = devm_reset_control_get_shared(dev, NULL);
662 	if (IS_ERR(csi2_dev->reset)) {
663 		dev_err(dev, "failed to get reset controller\n");
664 		return PTR_ERR(csi2_dev->reset);
665 	}
666 
667 	/* D-PHY */
668 
669 	csi2_dev->dphy = devm_phy_get(dev, "dphy");
670 	if (IS_ERR(csi2_dev->dphy)) {
671 		dev_err(dev, "failed to get MIPI D-PHY\n");
672 		return PTR_ERR(csi2_dev->dphy);
673 	}
674 
675 	ret = phy_init(csi2_dev->dphy);
676 	if (ret) {
677 		dev_err(dev, "failed to initialize MIPI D-PHY\n");
678 		return ret;
679 	}
680 
681 	/* Runtime PM */
682 
683 	pm_runtime_enable(dev);
684 
685 	return 0;
686 }
687 
688 static void
689 sun6i_mipi_csi2_resources_cleanup(struct sun6i_mipi_csi2_device *csi2_dev)
690 {
691 	pm_runtime_disable(csi2_dev->dev);
692 	phy_exit(csi2_dev->dphy);
693 	clk_rate_exclusive_put(csi2_dev->clock_mod);
694 }
695 
696 static int sun6i_mipi_csi2_probe(struct platform_device *platform_dev)
697 {
698 	struct sun6i_mipi_csi2_device *csi2_dev;
699 	struct device *dev = &platform_dev->dev;
700 	int ret;
701 
702 	csi2_dev = devm_kzalloc(dev, sizeof(*csi2_dev), GFP_KERNEL);
703 	if (!csi2_dev)
704 		return -ENOMEM;
705 
706 	csi2_dev->dev = dev;
707 	platform_set_drvdata(platform_dev, csi2_dev);
708 
709 	ret = sun6i_mipi_csi2_resources_setup(csi2_dev, platform_dev);
710 	if (ret)
711 		return ret;
712 
713 	ret = sun6i_mipi_csi2_bridge_setup(csi2_dev);
714 	if (ret)
715 		return ret;
716 
717 	return 0;
718 }
719 
720 static int sun6i_mipi_csi2_remove(struct platform_device *platform_dev)
721 {
722 	struct sun6i_mipi_csi2_device *csi2_dev =
723 		platform_get_drvdata(platform_dev);
724 
725 	sun6i_mipi_csi2_bridge_cleanup(csi2_dev);
726 	sun6i_mipi_csi2_resources_cleanup(csi2_dev);
727 
728 	return 0;
729 }
730 
731 static const struct of_device_id sun6i_mipi_csi2_of_match[] = {
732 	{ .compatible	= "allwinner,sun6i-a31-mipi-csi2" },
733 	{},
734 };
735 MODULE_DEVICE_TABLE(of, sun6i_mipi_csi2_of_match);
736 
737 static struct platform_driver sun6i_mipi_csi2_platform_driver = {
738 	.probe	= sun6i_mipi_csi2_probe,
739 	.remove	= sun6i_mipi_csi2_remove,
740 	.driver	= {
741 		.name		= SUN6I_MIPI_CSI2_NAME,
742 		.of_match_table	= of_match_ptr(sun6i_mipi_csi2_of_match),
743 		.pm		= &sun6i_mipi_csi2_pm_ops,
744 	},
745 };
746 module_platform_driver(sun6i_mipi_csi2_platform_driver);
747 
748 MODULE_DESCRIPTION("Allwinner A31 MIPI CSI-2 Controller Driver");
749 MODULE_AUTHOR("Paul Kocialkowski <paul.kocialkowski@bootlin.com>");
750 MODULE_LICENSE("GPL");
751