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