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 		v4l2_subdev_call(source_subdev, video, s_stream, 0);
192 		ret = 0;
193 		goto disable;
194 	}
195 
196 	/* Runtime PM */
197 
198 	ret = pm_runtime_resume_and_get(dev);
199 	if (ret < 0)
200 		return ret;
201 
202 	/* Sensor Pixel Rate */
203 
204 	ctrl = v4l2_ctrl_find(source_subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
205 	if (!ctrl) {
206 		dev_err(dev, "missing sensor pixel rate\n");
207 		ret = -ENODEV;
208 		goto error_pm;
209 	}
210 
211 	pixel_rate = (unsigned long)v4l2_ctrl_g_ctrl_int64(ctrl);
212 	if (!pixel_rate) {
213 		dev_err(dev, "missing (zero) sensor pixel rate\n");
214 		ret = -ENODEV;
215 		goto error_pm;
216 	}
217 
218 	/* D-PHY */
219 
220 	if (!lanes_count) {
221 		dev_err(dev, "missing (zero) MIPI CSI-2 lanes count\n");
222 		ret = -ENODEV;
223 		goto error_pm;
224 	}
225 
226 	format = sun6i_mipi_csi2_format_find(mbus_format->code);
227 	if (WARN_ON(!format)) {
228 		ret = -ENODEV;
229 		goto error_pm;
230 	}
231 
232 	phy_mipi_dphy_get_default_config(pixel_rate, format->bpp, lanes_count,
233 					 dphy_cfg);
234 
235 	/*
236 	 * Note that our hardware is using DDR, which is not taken in account by
237 	 * phy_mipi_dphy_get_default_config when calculating hs_clk_rate from
238 	 * the pixel rate, lanes count and bpp.
239 	 *
240 	 * The resulting clock rate is basically the symbol rate over the whole
241 	 * link. The actual clock rate is calculated with division by two since
242 	 * DDR samples both on rising and falling edges.
243 	 */
244 
245 	dev_dbg(dev, "A31 MIPI CSI-2 config:\n");
246 	dev_dbg(dev, "%ld pixels/s, %u bits/pixel, %u lanes, %lu Hz clock\n",
247 		pixel_rate, format->bpp, lanes_count,
248 		dphy_cfg->hs_clk_rate / 2);
249 
250 	ret = phy_reset(dphy);
251 	if (ret) {
252 		dev_err(dev, "failed to reset MIPI D-PHY\n");
253 		goto error_pm;
254 	}
255 
256 	ret = phy_configure(dphy, &dphy_opts);
257 	if (ret) {
258 		dev_err(dev, "failed to configure MIPI D-PHY\n");
259 		goto error_pm;
260 	}
261 
262 	/* Controller */
263 
264 	sun6i_mipi_csi2_configure(csi2_dev);
265 	sun6i_mipi_csi2_enable(csi2_dev);
266 
267 	/* D-PHY */
268 
269 	ret = phy_power_on(dphy);
270 	if (ret) {
271 		dev_err(dev, "failed to power on MIPI D-PHY\n");
272 		goto error_pm;
273 	}
274 
275 	/* Source */
276 
277 	ret = v4l2_subdev_call(source_subdev, video, s_stream, 1);
278 	if (ret && ret != -ENOIOCTLCMD)
279 		goto disable;
280 
281 	return 0;
282 
283 disable:
284 	phy_power_off(dphy);
285 	sun6i_mipi_csi2_disable(csi2_dev);
286 
287 error_pm:
288 	pm_runtime_put(dev);
289 
290 	return ret;
291 }
292 
293 static const struct v4l2_subdev_video_ops sun6i_mipi_csi2_video_ops = {
294 	.s_stream	= sun6i_mipi_csi2_s_stream,
295 };
296 
297 static void
298 sun6i_mipi_csi2_mbus_format_prepare(struct v4l2_mbus_framefmt *mbus_format)
299 {
300 	if (!sun6i_mipi_csi2_format_find(mbus_format->code))
301 		mbus_format->code = sun6i_mipi_csi2_formats[0].mbus_code;
302 
303 	mbus_format->field = V4L2_FIELD_NONE;
304 	mbus_format->colorspace = V4L2_COLORSPACE_RAW;
305 	mbus_format->quantization = V4L2_QUANTIZATION_DEFAULT;
306 	mbus_format->xfer_func = V4L2_XFER_FUNC_DEFAULT;
307 }
308 
309 static int sun6i_mipi_csi2_init_cfg(struct v4l2_subdev *subdev,
310 				    struct v4l2_subdev_state *state)
311 {
312 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
313 	unsigned int pad = SUN6I_MIPI_CSI2_PAD_SINK;
314 	struct v4l2_mbus_framefmt *mbus_format =
315 		v4l2_subdev_get_try_format(subdev, state, pad);
316 	struct mutex *lock = &csi2_dev->bridge.lock;
317 
318 	mutex_lock(lock);
319 
320 	mbus_format->code = sun6i_mipi_csi2_formats[0].mbus_code;
321 	mbus_format->width = 640;
322 	mbus_format->height = 480;
323 
324 	sun6i_mipi_csi2_mbus_format_prepare(mbus_format);
325 
326 	mutex_unlock(lock);
327 
328 	return 0;
329 }
330 
331 static int
332 sun6i_mipi_csi2_enum_mbus_code(struct v4l2_subdev *subdev,
333 			       struct v4l2_subdev_state *state,
334 			       struct v4l2_subdev_mbus_code_enum *code_enum)
335 {
336 	if (code_enum->index >= ARRAY_SIZE(sun6i_mipi_csi2_formats))
337 		return -EINVAL;
338 
339 	code_enum->code = sun6i_mipi_csi2_formats[code_enum->index].mbus_code;
340 
341 	return 0;
342 }
343 
344 static int sun6i_mipi_csi2_get_fmt(struct v4l2_subdev *subdev,
345 				   struct v4l2_subdev_state *state,
346 				   struct v4l2_subdev_format *format)
347 {
348 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
349 	struct v4l2_mbus_framefmt *mbus_format = &format->format;
350 	struct mutex *lock = &csi2_dev->bridge.lock;
351 
352 	mutex_lock(lock);
353 
354 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
355 		*mbus_format = *v4l2_subdev_get_try_format(subdev, state,
356 							   format->pad);
357 	else
358 		*mbus_format = csi2_dev->bridge.mbus_format;
359 
360 	mutex_unlock(lock);
361 
362 	return 0;
363 }
364 
365 static int sun6i_mipi_csi2_set_fmt(struct v4l2_subdev *subdev,
366 				   struct v4l2_subdev_state *state,
367 				   struct v4l2_subdev_format *format)
368 {
369 	struct sun6i_mipi_csi2_device *csi2_dev = v4l2_get_subdevdata(subdev);
370 	struct v4l2_mbus_framefmt *mbus_format = &format->format;
371 	struct mutex *lock = &csi2_dev->bridge.lock;
372 
373 	mutex_lock(lock);
374 
375 	sun6i_mipi_csi2_mbus_format_prepare(mbus_format);
376 
377 	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
378 		*v4l2_subdev_get_try_format(subdev, state, format->pad) =
379 			*mbus_format;
380 	else
381 		csi2_dev->bridge.mbus_format = *mbus_format;
382 
383 	mutex_unlock(lock);
384 
385 	return 0;
386 }
387 
388 static const struct v4l2_subdev_pad_ops sun6i_mipi_csi2_pad_ops = {
389 	.init_cfg	= sun6i_mipi_csi2_init_cfg,
390 	.enum_mbus_code	= sun6i_mipi_csi2_enum_mbus_code,
391 	.get_fmt	= sun6i_mipi_csi2_get_fmt,
392 	.set_fmt	= sun6i_mipi_csi2_set_fmt,
393 };
394 
395 static const struct v4l2_subdev_ops sun6i_mipi_csi2_subdev_ops = {
396 	.video	= &sun6i_mipi_csi2_video_ops,
397 	.pad	= &sun6i_mipi_csi2_pad_ops,
398 };
399 
400 /* Media Entity */
401 
402 static const struct media_entity_operations sun6i_mipi_csi2_entity_ops = {
403 	.link_validate	= v4l2_subdev_link_validate,
404 };
405 
406 /* V4L2 Async */
407 
408 static int
409 sun6i_mipi_csi2_notifier_bound(struct v4l2_async_notifier *notifier,
410 			       struct v4l2_subdev *remote_subdev,
411 			       struct v4l2_async_subdev *async_subdev)
412 {
413 	struct v4l2_subdev *subdev = notifier->sd;
414 	struct sun6i_mipi_csi2_device *csi2_dev =
415 		container_of(notifier, struct sun6i_mipi_csi2_device,
416 			     bridge.notifier);
417 	struct media_entity *sink_entity = &subdev->entity;
418 	struct media_entity *source_entity = &remote_subdev->entity;
419 	struct device *dev = csi2_dev->dev;
420 	int sink_pad_index = 0;
421 	int source_pad_index;
422 	int ret;
423 
424 	ret = media_entity_get_fwnode_pad(source_entity, remote_subdev->fwnode,
425 					  MEDIA_PAD_FL_SOURCE);
426 	if (ret < 0) {
427 		dev_err(dev, "missing source pad in external entity %s\n",
428 			source_entity->name);
429 		return -EINVAL;
430 	}
431 
432 	source_pad_index = ret;
433 
434 	dev_dbg(dev, "creating %s:%u -> %s:%u link\n", source_entity->name,
435 		source_pad_index, sink_entity->name, sink_pad_index);
436 
437 	ret = media_create_pad_link(source_entity, source_pad_index,
438 				    sink_entity, sink_pad_index,
439 				    MEDIA_LNK_FL_ENABLED |
440 				    MEDIA_LNK_FL_IMMUTABLE);
441 	if (ret) {
442 		dev_err(dev, "failed to create %s:%u -> %s:%u link\n",
443 			source_entity->name, source_pad_index,
444 			sink_entity->name, sink_pad_index);
445 		return ret;
446 	}
447 
448 	csi2_dev->bridge.source_subdev = remote_subdev;
449 
450 	return 0;
451 }
452 
453 static const struct v4l2_async_notifier_operations
454 sun6i_mipi_csi2_notifier_ops = {
455 	.bound	= sun6i_mipi_csi2_notifier_bound,
456 };
457 
458 /* Bridge */
459 
460 static int
461 sun6i_mipi_csi2_bridge_source_setup(struct sun6i_mipi_csi2_device *csi2_dev)
462 {
463 	struct v4l2_async_notifier *notifier = &csi2_dev->bridge.notifier;
464 	struct v4l2_fwnode_endpoint *endpoint = &csi2_dev->bridge.endpoint;
465 	struct v4l2_async_subdev *subdev_async;
466 	struct fwnode_handle *handle;
467 	struct device *dev = csi2_dev->dev;
468 	int ret;
469 
470 	handle = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), 0, 0,
471 						 FWNODE_GRAPH_ENDPOINT_NEXT);
472 	if (!handle)
473 		return -ENODEV;
474 
475 	endpoint->bus_type = V4L2_MBUS_CSI2_DPHY;
476 
477 	ret = v4l2_fwnode_endpoint_parse(handle, endpoint);
478 	if (ret)
479 		goto complete;
480 
481 	subdev_async =
482 		v4l2_async_nf_add_fwnode_remote(notifier, handle,
483 						struct v4l2_async_subdev);
484 	if (IS_ERR(subdev_async))
485 		ret = PTR_ERR(subdev_async);
486 
487 complete:
488 	fwnode_handle_put(handle);
489 
490 	return ret;
491 }
492 
493 static int sun6i_mipi_csi2_bridge_setup(struct sun6i_mipi_csi2_device *csi2_dev)
494 {
495 	struct sun6i_mipi_csi2_bridge *bridge = &csi2_dev->bridge;
496 	struct v4l2_subdev *subdev = &bridge->subdev;
497 	struct v4l2_async_notifier *notifier = &bridge->notifier;
498 	struct media_pad *pads = bridge->pads;
499 	struct device *dev = csi2_dev->dev;
500 	bool notifier_registered = false;
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 					       MEDIA_PAD_FL_MUST_CONNECT;
524 	pads[SUN6I_MIPI_CSI2_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE |
525 						 MEDIA_PAD_FL_MUST_CONNECT;
526 
527 	ret = media_entity_pads_init(&subdev->entity, SUN6I_MIPI_CSI2_PAD_COUNT,
528 				     pads);
529 	if (ret)
530 		return ret;
531 
532 	/* V4L2 Async */
533 
534 	v4l2_async_nf_init(notifier);
535 	notifier->ops = &sun6i_mipi_csi2_notifier_ops;
536 
537 	ret = sun6i_mipi_csi2_bridge_source_setup(csi2_dev);
538 	if (ret && ret != -ENODEV)
539 		goto error_v4l2_notifier_cleanup;
540 
541 	/* Only register the notifier when a sensor is connected. */
542 	if (ret != -ENODEV) {
543 		ret = v4l2_async_subdev_nf_register(subdev, notifier);
544 		if (ret < 0)
545 			goto error_v4l2_notifier_cleanup;
546 
547 		notifier_registered = true;
548 	}
549 
550 	/* V4L2 Subdev */
551 
552 	ret = v4l2_async_register_subdev(subdev);
553 	if (ret < 0)
554 		goto error_v4l2_notifier_unregister;
555 
556 	return 0;
557 
558 error_v4l2_notifier_unregister:
559 	if (notifier_registered)
560 		v4l2_async_nf_unregister(notifier);
561 
562 error_v4l2_notifier_cleanup:
563 	v4l2_async_nf_cleanup(notifier);
564 
565 	media_entity_cleanup(&subdev->entity);
566 
567 	return ret;
568 }
569 
570 static void
571 sun6i_mipi_csi2_bridge_cleanup(struct sun6i_mipi_csi2_device *csi2_dev)
572 {
573 	struct v4l2_subdev *subdev = &csi2_dev->bridge.subdev;
574 	struct v4l2_async_notifier *notifier = &csi2_dev->bridge.notifier;
575 
576 	v4l2_async_unregister_subdev(subdev);
577 	v4l2_async_nf_unregister(notifier);
578 	v4l2_async_nf_cleanup(notifier);
579 	media_entity_cleanup(&subdev->entity);
580 }
581 
582 /* Platform */
583 
584 static int sun6i_mipi_csi2_suspend(struct device *dev)
585 {
586 	struct sun6i_mipi_csi2_device *csi2_dev = dev_get_drvdata(dev);
587 
588 	clk_disable_unprepare(csi2_dev->clock_mod);
589 	reset_control_assert(csi2_dev->reset);
590 
591 	return 0;
592 }
593 
594 static int sun6i_mipi_csi2_resume(struct device *dev)
595 {
596 	struct sun6i_mipi_csi2_device *csi2_dev = dev_get_drvdata(dev);
597 	int ret;
598 
599 	ret = reset_control_deassert(csi2_dev->reset);
600 	if (ret) {
601 		dev_err(dev, "failed to deassert reset\n");
602 		return ret;
603 	}
604 
605 	ret = clk_prepare_enable(csi2_dev->clock_mod);
606 	if (ret) {
607 		dev_err(dev, "failed to enable module clock\n");
608 		goto error_reset;
609 	}
610 
611 	return 0;
612 
613 error_reset:
614 	reset_control_assert(csi2_dev->reset);
615 
616 	return ret;
617 }
618 
619 static const struct dev_pm_ops sun6i_mipi_csi2_pm_ops = {
620 	.runtime_suspend	= sun6i_mipi_csi2_suspend,
621 	.runtime_resume		= sun6i_mipi_csi2_resume,
622 };
623 
624 static const struct regmap_config sun6i_mipi_csi2_regmap_config = {
625 	.reg_bits       = 32,
626 	.reg_stride     = 4,
627 	.val_bits       = 32,
628 	.max_register	= 0x400,
629 };
630 
631 static int
632 sun6i_mipi_csi2_resources_setup(struct sun6i_mipi_csi2_device *csi2_dev,
633 				struct platform_device *platform_dev)
634 {
635 	struct device *dev = csi2_dev->dev;
636 	void __iomem *io_base;
637 	int ret;
638 
639 	/* Registers */
640 
641 	io_base = devm_platform_ioremap_resource(platform_dev, 0);
642 	if (IS_ERR(io_base))
643 		return PTR_ERR(io_base);
644 
645 	csi2_dev->regmap =
646 		devm_regmap_init_mmio_clk(dev, "bus", io_base,
647 					  &sun6i_mipi_csi2_regmap_config);
648 	if (IS_ERR(csi2_dev->regmap)) {
649 		dev_err(dev, "failed to init register map\n");
650 		return PTR_ERR(csi2_dev->regmap);
651 	}
652 
653 	/* Clock */
654 
655 	csi2_dev->clock_mod = devm_clk_get(dev, "mod");
656 	if (IS_ERR(csi2_dev->clock_mod)) {
657 		dev_err(dev, "failed to acquire mod clock\n");
658 		return PTR_ERR(csi2_dev->clock_mod);
659 	}
660 
661 	ret = clk_set_rate_exclusive(csi2_dev->clock_mod, 297000000);
662 	if (ret) {
663 		dev_err(dev, "failed to set mod clock rate\n");
664 		return ret;
665 	}
666 
667 	/* Reset */
668 
669 	csi2_dev->reset = devm_reset_control_get_shared(dev, NULL);
670 	if (IS_ERR(csi2_dev->reset)) {
671 		dev_err(dev, "failed to get reset controller\n");
672 		ret = PTR_ERR(csi2_dev->reset);
673 		goto error_clock_rate_exclusive;
674 	}
675 
676 	/* D-PHY */
677 
678 	csi2_dev->dphy = devm_phy_get(dev, "dphy");
679 	if (IS_ERR(csi2_dev->dphy)) {
680 		dev_err(dev, "failed to get MIPI D-PHY\n");
681 		ret = PTR_ERR(csi2_dev->dphy);
682 		goto error_clock_rate_exclusive;
683 	}
684 
685 	ret = phy_init(csi2_dev->dphy);
686 	if (ret) {
687 		dev_err(dev, "failed to initialize MIPI D-PHY\n");
688 		goto error_clock_rate_exclusive;
689 	}
690 
691 	/* Runtime PM */
692 
693 	pm_runtime_enable(dev);
694 
695 	return 0;
696 
697 error_clock_rate_exclusive:
698 	clk_rate_exclusive_put(csi2_dev->clock_mod);
699 
700 	return ret;
701 }
702 
703 static void
704 sun6i_mipi_csi2_resources_cleanup(struct sun6i_mipi_csi2_device *csi2_dev)
705 {
706 	pm_runtime_disable(csi2_dev->dev);
707 	phy_exit(csi2_dev->dphy);
708 	clk_rate_exclusive_put(csi2_dev->clock_mod);
709 }
710 
711 static int sun6i_mipi_csi2_probe(struct platform_device *platform_dev)
712 {
713 	struct sun6i_mipi_csi2_device *csi2_dev;
714 	struct device *dev = &platform_dev->dev;
715 	int ret;
716 
717 	csi2_dev = devm_kzalloc(dev, sizeof(*csi2_dev), GFP_KERNEL);
718 	if (!csi2_dev)
719 		return -ENOMEM;
720 
721 	csi2_dev->dev = dev;
722 	platform_set_drvdata(platform_dev, csi2_dev);
723 
724 	ret = sun6i_mipi_csi2_resources_setup(csi2_dev, platform_dev);
725 	if (ret)
726 		return ret;
727 
728 	ret = sun6i_mipi_csi2_bridge_setup(csi2_dev);
729 	if (ret)
730 		goto error_resources;
731 
732 	return 0;
733 
734 error_resources:
735 	sun6i_mipi_csi2_resources_cleanup(csi2_dev);
736 
737 	return ret;
738 }
739 
740 static void sun6i_mipi_csi2_remove(struct platform_device *platform_dev)
741 {
742 	struct sun6i_mipi_csi2_device *csi2_dev =
743 		platform_get_drvdata(platform_dev);
744 
745 	sun6i_mipi_csi2_bridge_cleanup(csi2_dev);
746 	sun6i_mipi_csi2_resources_cleanup(csi2_dev);
747 }
748 
749 static const struct of_device_id sun6i_mipi_csi2_of_match[] = {
750 	{ .compatible	= "allwinner,sun6i-a31-mipi-csi2" },
751 	{},
752 };
753 MODULE_DEVICE_TABLE(of, sun6i_mipi_csi2_of_match);
754 
755 static struct platform_driver sun6i_mipi_csi2_platform_driver = {
756 	.probe	= sun6i_mipi_csi2_probe,
757 	.remove_new = sun6i_mipi_csi2_remove,
758 	.driver	= {
759 		.name		= SUN6I_MIPI_CSI2_NAME,
760 		.of_match_table	= of_match_ptr(sun6i_mipi_csi2_of_match),
761 		.pm		= &sun6i_mipi_csi2_pm_ops,
762 	},
763 };
764 module_platform_driver(sun6i_mipi_csi2_platform_driver);
765 
766 MODULE_DESCRIPTION("Allwinner A31 MIPI CSI-2 Controller Driver");
767 MODULE_AUTHOR("Paul Kocialkowski <paul.kocialkowski@bootlin.com>");
768 MODULE_LICENSE("GPL");
769