1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * camss-csiphy.c
4  *
5  * Qualcomm MSM Camera Subsystem - CSIPHY Module
6  *
7  * Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
8  * Copyright (C) 2016-2018 Linaro Ltd.
9  */
10 #include <linux/clk.h>
11 #include <linux/delay.h>
12 #include <linux/interrupt.h>
13 #include <linux/io.h>
14 #include <linux/kernel.h>
15 #include <linux/of.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <media/media-entity.h>
19 #include <media/v4l2-device.h>
20 #include <media/v4l2-subdev.h>
21 
22 #include "camss-csiphy.h"
23 #include "camss.h"
24 
25 #define MSM_CSIPHY_NAME "msm_csiphy"
26 
27 struct csiphy_format {
28 	u32 code;
29 	u8 bpp;
30 };
31 
32 static const struct csiphy_format csiphy_formats_8x16[] = {
33 	{ MEDIA_BUS_FMT_UYVY8_2X8, 8 },
34 	{ MEDIA_BUS_FMT_VYUY8_2X8, 8 },
35 	{ MEDIA_BUS_FMT_YUYV8_2X8, 8 },
36 	{ MEDIA_BUS_FMT_YVYU8_2X8, 8 },
37 	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
38 	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
39 	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
40 	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
41 	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
42 	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
43 	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
44 	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
45 	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
46 	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
47 	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
48 	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
49 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
50 };
51 
52 static const struct csiphy_format csiphy_formats_8x96[] = {
53 	{ MEDIA_BUS_FMT_UYVY8_2X8, 8 },
54 	{ MEDIA_BUS_FMT_VYUY8_2X8, 8 },
55 	{ MEDIA_BUS_FMT_YUYV8_2X8, 8 },
56 	{ MEDIA_BUS_FMT_YVYU8_2X8, 8 },
57 	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
58 	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
59 	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
60 	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
61 	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
62 	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
63 	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
64 	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
65 	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
66 	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
67 	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
68 	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
69 	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
70 	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
71 	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
72 	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
73 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
74 };
75 
76 static const struct csiphy_format csiphy_formats_sdm845[] = {
77 	{ MEDIA_BUS_FMT_UYVY8_2X8, 8 },
78 	{ MEDIA_BUS_FMT_VYUY8_2X8, 8 },
79 	{ MEDIA_BUS_FMT_YUYV8_2X8, 8 },
80 	{ MEDIA_BUS_FMT_YVYU8_2X8, 8 },
81 	{ MEDIA_BUS_FMT_SBGGR8_1X8, 8 },
82 	{ MEDIA_BUS_FMT_SGBRG8_1X8, 8 },
83 	{ MEDIA_BUS_FMT_SGRBG8_1X8, 8 },
84 	{ MEDIA_BUS_FMT_SRGGB8_1X8, 8 },
85 	{ MEDIA_BUS_FMT_SBGGR10_1X10, 10 },
86 	{ MEDIA_BUS_FMT_SGBRG10_1X10, 10 },
87 	{ MEDIA_BUS_FMT_SGRBG10_1X10, 10 },
88 	{ MEDIA_BUS_FMT_SRGGB10_1X10, 10 },
89 	{ MEDIA_BUS_FMT_SBGGR12_1X12, 12 },
90 	{ MEDIA_BUS_FMT_SGBRG12_1X12, 12 },
91 	{ MEDIA_BUS_FMT_SGRBG12_1X12, 12 },
92 	{ MEDIA_BUS_FMT_SRGGB12_1X12, 12 },
93 	{ MEDIA_BUS_FMT_SBGGR14_1X14, 14 },
94 	{ MEDIA_BUS_FMT_SGBRG14_1X14, 14 },
95 	{ MEDIA_BUS_FMT_SGRBG14_1X14, 14 },
96 	{ MEDIA_BUS_FMT_SRGGB14_1X14, 14 },
97 	{ MEDIA_BUS_FMT_Y8_1X8, 8 },
98 	{ MEDIA_BUS_FMT_Y10_1X10, 10 },
99 };
100 
101 /*
102  * csiphy_get_bpp - map media bus format to bits per pixel
103  * @formats: supported media bus formats array
104  * @nformats: size of @formats array
105  * @code: media bus format code
106  *
107  * Return number of bits per pixel
108  */
109 static u8 csiphy_get_bpp(const struct csiphy_format *formats,
110 			 unsigned int nformats, u32 code)
111 {
112 	unsigned int i;
113 
114 	for (i = 0; i < nformats; i++)
115 		if (code == formats[i].code)
116 			return formats[i].bpp;
117 
118 	WARN(1, "Unknown format\n");
119 
120 	return formats[0].bpp;
121 }
122 
123 /*
124  * csiphy_set_clock_rates - Calculate and set clock rates on CSIPHY module
125  * @csiphy: CSIPHY device
126  */
127 static int csiphy_set_clock_rates(struct csiphy_device *csiphy)
128 {
129 	struct device *dev = csiphy->camss->dev;
130 	s64 link_freq;
131 	int i, j;
132 	int ret;
133 
134 	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
135 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
136 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
137 
138 	link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
139 	if (link_freq < 0)
140 		link_freq  = 0;
141 
142 	for (i = 0; i < csiphy->nclocks; i++) {
143 		struct camss_clock *clock = &csiphy->clock[i];
144 
145 		if (csiphy->rate_set[i]) {
146 			u64 min_rate = link_freq / 4;
147 			long round_rate;
148 
149 			camss_add_clock_margin(&min_rate);
150 
151 			for (j = 0; j < clock->nfreqs; j++)
152 				if (min_rate < clock->freq[j])
153 					break;
154 
155 			if (j == clock->nfreqs) {
156 				dev_err(dev,
157 					"Pixel clock is too high for CSIPHY\n");
158 				return -EINVAL;
159 			}
160 
161 			/* if sensor pixel clock is not available */
162 			/* set highest possible CSIPHY clock rate */
163 			if (min_rate == 0)
164 				j = clock->nfreqs - 1;
165 
166 			round_rate = clk_round_rate(clock->clk, clock->freq[j]);
167 			if (round_rate < 0) {
168 				dev_err(dev, "clk round rate failed: %ld\n",
169 					round_rate);
170 				return -EINVAL;
171 			}
172 
173 			csiphy->timer_clk_rate = round_rate;
174 
175 			ret = clk_set_rate(clock->clk, csiphy->timer_clk_rate);
176 			if (ret < 0) {
177 				dev_err(dev, "clk set rate failed: %d\n", ret);
178 				return ret;
179 			}
180 		}
181 	}
182 
183 	return 0;
184 }
185 
186 /*
187  * csiphy_set_power - Power on/off CSIPHY module
188  * @sd: CSIPHY V4L2 subdevice
189  * @on: Requested power state
190  *
191  * Return 0 on success or a negative error code otherwise
192  */
193 static int csiphy_set_power(struct v4l2_subdev *sd, int on)
194 {
195 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
196 	struct device *dev = csiphy->camss->dev;
197 
198 	if (on) {
199 		int ret;
200 
201 		ret = pm_runtime_resume_and_get(dev);
202 		if (ret < 0)
203 			return ret;
204 
205 		ret = csiphy_set_clock_rates(csiphy);
206 		if (ret < 0) {
207 			pm_runtime_put_sync(dev);
208 			return ret;
209 		}
210 
211 		ret = camss_enable_clocks(csiphy->nclocks, csiphy->clock, dev);
212 		if (ret < 0) {
213 			pm_runtime_put_sync(dev);
214 			return ret;
215 		}
216 
217 		enable_irq(csiphy->irq);
218 
219 		csiphy->ops->reset(csiphy);
220 
221 		csiphy->ops->hw_version_read(csiphy, dev);
222 	} else {
223 		disable_irq(csiphy->irq);
224 
225 		camss_disable_clocks(csiphy->nclocks, csiphy->clock);
226 
227 		pm_runtime_put_sync(dev);
228 	}
229 
230 	return 0;
231 }
232 
233 /*
234  * csiphy_stream_on - Enable streaming on CSIPHY module
235  * @csiphy: CSIPHY device
236  *
237  * Helper function to enable streaming on CSIPHY module.
238  * Main configuration of CSIPHY module is also done here.
239  *
240  * Return 0 on success or a negative error code otherwise
241  */
242 static int csiphy_stream_on(struct csiphy_device *csiphy)
243 {
244 	struct csiphy_config *cfg = &csiphy->cfg;
245 	s64 link_freq;
246 	u8 lane_mask = csiphy->ops->get_lane_mask(&cfg->csi2->lane_cfg);
247 	u8 bpp = csiphy_get_bpp(csiphy->formats, csiphy->nformats,
248 				csiphy->fmt[MSM_CSIPHY_PAD_SINK].code);
249 	u8 num_lanes = csiphy->cfg.csi2->lane_cfg.num_data;
250 	u8 val;
251 
252 	link_freq = camss_get_link_freq(&csiphy->subdev.entity, bpp, num_lanes);
253 
254 	if (link_freq < 0) {
255 		dev_err(csiphy->camss->dev,
256 			"Cannot get CSI2 transmitter's link frequency\n");
257 		return -EINVAL;
258 	}
259 
260 	if (csiphy->base_clk_mux) {
261 		val = readl_relaxed(csiphy->base_clk_mux);
262 		if (cfg->combo_mode && (lane_mask & 0x18) == 0x18) {
263 			val &= ~0xf0;
264 			val |= cfg->csid_id << 4;
265 		} else {
266 			val &= ~0xf;
267 			val |= cfg->csid_id;
268 		}
269 		writel_relaxed(val, csiphy->base_clk_mux);
270 
271 		/* Enforce reg write ordering between clk mux & lane enabling */
272 		wmb();
273 	}
274 
275 	csiphy->ops->lanes_enable(csiphy, cfg, link_freq, lane_mask);
276 
277 	return 0;
278 }
279 
280 /*
281  * csiphy_stream_off - Disable streaming on CSIPHY module
282  * @csiphy: CSIPHY device
283  *
284  * Helper function to disable streaming on CSIPHY module
285  */
286 static void csiphy_stream_off(struct csiphy_device *csiphy)
287 {
288 	csiphy->ops->lanes_disable(csiphy, &csiphy->cfg);
289 }
290 
291 
292 /*
293  * csiphy_set_stream - Enable/disable streaming on CSIPHY module
294  * @sd: CSIPHY V4L2 subdevice
295  * @enable: Requested streaming state
296  *
297  * Return 0 on success or a negative error code otherwise
298  */
299 static int csiphy_set_stream(struct v4l2_subdev *sd, int enable)
300 {
301 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
302 	int ret = 0;
303 
304 	if (enable)
305 		ret = csiphy_stream_on(csiphy);
306 	else
307 		csiphy_stream_off(csiphy);
308 
309 	return ret;
310 }
311 
312 /*
313  * __csiphy_get_format - Get pointer to format structure
314  * @csiphy: CSIPHY device
315  * @cfg: V4L2 subdev pad configuration
316  * @pad: pad from which format is requested
317  * @which: TRY or ACTIVE format
318  *
319  * Return pointer to TRY or ACTIVE format structure
320  */
321 static struct v4l2_mbus_framefmt *
322 __csiphy_get_format(struct csiphy_device *csiphy,
323 		    struct v4l2_subdev_state *sd_state,
324 		    unsigned int pad,
325 		    enum v4l2_subdev_format_whence which)
326 {
327 	if (which == V4L2_SUBDEV_FORMAT_TRY)
328 		return v4l2_subdev_get_try_format(&csiphy->subdev, sd_state,
329 						  pad);
330 
331 	return &csiphy->fmt[pad];
332 }
333 
334 /*
335  * csiphy_try_format - Handle try format by pad subdev method
336  * @csiphy: CSIPHY device
337  * @cfg: V4L2 subdev pad configuration
338  * @pad: pad on which format is requested
339  * @fmt: pointer to v4l2 format structure
340  * @which: wanted subdev format
341  */
342 static void csiphy_try_format(struct csiphy_device *csiphy,
343 			      struct v4l2_subdev_state *sd_state,
344 			      unsigned int pad,
345 			      struct v4l2_mbus_framefmt *fmt,
346 			      enum v4l2_subdev_format_whence which)
347 {
348 	unsigned int i;
349 
350 	switch (pad) {
351 	case MSM_CSIPHY_PAD_SINK:
352 		/* Set format on sink pad */
353 
354 		for (i = 0; i < csiphy->nformats; i++)
355 			if (fmt->code == csiphy->formats[i].code)
356 				break;
357 
358 		/* If not found, use UYVY as default */
359 		if (i >= csiphy->nformats)
360 			fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;
361 
362 		fmt->width = clamp_t(u32, fmt->width, 1, 8191);
363 		fmt->height = clamp_t(u32, fmt->height, 1, 8191);
364 
365 		fmt->field = V4L2_FIELD_NONE;
366 		fmt->colorspace = V4L2_COLORSPACE_SRGB;
367 
368 		break;
369 
370 	case MSM_CSIPHY_PAD_SRC:
371 		/* Set and return a format same as sink pad */
372 
373 		*fmt = *__csiphy_get_format(csiphy, sd_state,
374 					    MSM_CSID_PAD_SINK,
375 					    which);
376 
377 		break;
378 	}
379 }
380 
381 /*
382  * csiphy_enum_mbus_code - Handle pixel format enumeration
383  * @sd: CSIPHY V4L2 subdevice
384  * @cfg: V4L2 subdev pad configuration
385  * @code: pointer to v4l2_subdev_mbus_code_enum structure
386  * return -EINVAL or zero on success
387  */
388 static int csiphy_enum_mbus_code(struct v4l2_subdev *sd,
389 				 struct v4l2_subdev_state *sd_state,
390 				 struct v4l2_subdev_mbus_code_enum *code)
391 {
392 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
393 	struct v4l2_mbus_framefmt *format;
394 
395 	if (code->pad == MSM_CSIPHY_PAD_SINK) {
396 		if (code->index >= csiphy->nformats)
397 			return -EINVAL;
398 
399 		code->code = csiphy->formats[code->index].code;
400 	} else {
401 		if (code->index > 0)
402 			return -EINVAL;
403 
404 		format = __csiphy_get_format(csiphy, sd_state,
405 					     MSM_CSIPHY_PAD_SINK,
406 					     code->which);
407 
408 		code->code = format->code;
409 	}
410 
411 	return 0;
412 }
413 
414 /*
415  * csiphy_enum_frame_size - Handle frame size enumeration
416  * @sd: CSIPHY V4L2 subdevice
417  * @cfg: V4L2 subdev pad configuration
418  * @fse: pointer to v4l2_subdev_frame_size_enum structure
419  * return -EINVAL or zero on success
420  */
421 static int csiphy_enum_frame_size(struct v4l2_subdev *sd,
422 				  struct v4l2_subdev_state *sd_state,
423 				  struct v4l2_subdev_frame_size_enum *fse)
424 {
425 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
426 	struct v4l2_mbus_framefmt format;
427 
428 	if (fse->index != 0)
429 		return -EINVAL;
430 
431 	format.code = fse->code;
432 	format.width = 1;
433 	format.height = 1;
434 	csiphy_try_format(csiphy, sd_state, fse->pad, &format, fse->which);
435 	fse->min_width = format.width;
436 	fse->min_height = format.height;
437 
438 	if (format.code != fse->code)
439 		return -EINVAL;
440 
441 	format.code = fse->code;
442 	format.width = -1;
443 	format.height = -1;
444 	csiphy_try_format(csiphy, sd_state, fse->pad, &format, fse->which);
445 	fse->max_width = format.width;
446 	fse->max_height = format.height;
447 
448 	return 0;
449 }
450 
451 /*
452  * csiphy_get_format - Handle get format by pads subdev method
453  * @sd: CSIPHY V4L2 subdevice
454  * @cfg: V4L2 subdev pad configuration
455  * @fmt: pointer to v4l2 subdev format structure
456  *
457  * Return -EINVAL or zero on success
458  */
459 static int csiphy_get_format(struct v4l2_subdev *sd,
460 			     struct v4l2_subdev_state *sd_state,
461 			     struct v4l2_subdev_format *fmt)
462 {
463 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
464 	struct v4l2_mbus_framefmt *format;
465 
466 	format = __csiphy_get_format(csiphy, sd_state, fmt->pad, fmt->which);
467 	if (format == NULL)
468 		return -EINVAL;
469 
470 	fmt->format = *format;
471 
472 	return 0;
473 }
474 
475 /*
476  * csiphy_set_format - Handle set format by pads subdev method
477  * @sd: CSIPHY V4L2 subdevice
478  * @cfg: V4L2 subdev pad configuration
479  * @fmt: pointer to v4l2 subdev format structure
480  *
481  * Return -EINVAL or zero on success
482  */
483 static int csiphy_set_format(struct v4l2_subdev *sd,
484 			     struct v4l2_subdev_state *sd_state,
485 			     struct v4l2_subdev_format *fmt)
486 {
487 	struct csiphy_device *csiphy = v4l2_get_subdevdata(sd);
488 	struct v4l2_mbus_framefmt *format;
489 
490 	format = __csiphy_get_format(csiphy, sd_state, fmt->pad, fmt->which);
491 	if (format == NULL)
492 		return -EINVAL;
493 
494 	csiphy_try_format(csiphy, sd_state, fmt->pad, &fmt->format,
495 			  fmt->which);
496 	*format = fmt->format;
497 
498 	/* Propagate the format from sink to source */
499 	if (fmt->pad == MSM_CSIPHY_PAD_SINK) {
500 		format = __csiphy_get_format(csiphy, sd_state,
501 					     MSM_CSIPHY_PAD_SRC,
502 					     fmt->which);
503 
504 		*format = fmt->format;
505 		csiphy_try_format(csiphy, sd_state, MSM_CSIPHY_PAD_SRC,
506 				  format,
507 				  fmt->which);
508 	}
509 
510 	return 0;
511 }
512 
513 /*
514  * csiphy_init_formats - Initialize formats on all pads
515  * @sd: CSIPHY V4L2 subdevice
516  * @fh: V4L2 subdev file handle
517  *
518  * Initialize all pad formats with default values.
519  *
520  * Return 0 on success or a negative error code otherwise
521  */
522 static int csiphy_init_formats(struct v4l2_subdev *sd,
523 			       struct v4l2_subdev_fh *fh)
524 {
525 	struct v4l2_subdev_format format = {
526 		.pad = MSM_CSIPHY_PAD_SINK,
527 		.which = fh ? V4L2_SUBDEV_FORMAT_TRY :
528 			      V4L2_SUBDEV_FORMAT_ACTIVE,
529 		.format = {
530 			.code = MEDIA_BUS_FMT_UYVY8_2X8,
531 			.width = 1920,
532 			.height = 1080
533 		}
534 	};
535 
536 	return csiphy_set_format(sd, fh ? fh->state : NULL, &format);
537 }
538 
539 /*
540  * msm_csiphy_subdev_init - Initialize CSIPHY device structure and resources
541  * @csiphy: CSIPHY device
542  * @res: CSIPHY module resources table
543  * @id: CSIPHY module id
544  *
545  * Return 0 on success or a negative error code otherwise
546  */
547 int msm_csiphy_subdev_init(struct camss *camss,
548 			   struct csiphy_device *csiphy,
549 			   const struct resources *res, u8 id)
550 {
551 	struct device *dev = camss->dev;
552 	struct platform_device *pdev = to_platform_device(dev);
553 	int i, j;
554 	int ret;
555 
556 	csiphy->camss = camss;
557 	csiphy->id = id;
558 	csiphy->cfg.combo_mode = 0;
559 
560 	if (camss->version == CAMSS_8x16) {
561 		csiphy->ops = &csiphy_ops_2ph_1_0;
562 		csiphy->formats = csiphy_formats_8x16;
563 		csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x16);
564 	} else if (camss->version == CAMSS_8x96 ||
565 		   camss->version == CAMSS_660) {
566 		csiphy->ops = &csiphy_ops_3ph_1_0;
567 		csiphy->formats = csiphy_formats_8x96;
568 		csiphy->nformats = ARRAY_SIZE(csiphy_formats_8x96);
569 	} else if (camss->version == CAMSS_845 ||
570 		   camss->version == CAMSS_8250) {
571 		csiphy->ops = &csiphy_ops_3ph_1_0;
572 		csiphy->formats = csiphy_formats_sdm845;
573 		csiphy->nformats = ARRAY_SIZE(csiphy_formats_sdm845);
574 	} else {
575 		return -EINVAL;
576 	}
577 
578 	/* Memory */
579 
580 	csiphy->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);
581 	if (IS_ERR(csiphy->base))
582 		return PTR_ERR(csiphy->base);
583 
584 	if (camss->version == CAMSS_8x16 ||
585 	    camss->version == CAMSS_8x96) {
586 		csiphy->base_clk_mux =
587 			devm_platform_ioremap_resource_byname(pdev, res->reg[1]);
588 		if (IS_ERR(csiphy->base_clk_mux))
589 			return PTR_ERR(csiphy->base_clk_mux);
590 	} else {
591 		csiphy->base_clk_mux = NULL;
592 	}
593 
594 	/* Interrupt */
595 
596 	ret = platform_get_irq_byname(pdev, res->interrupt[0]);
597 	if (ret < 0)
598 		return ret;
599 
600 	csiphy->irq = ret;
601 	snprintf(csiphy->irq_name, sizeof(csiphy->irq_name), "%s_%s%d",
602 		 dev_name(dev), MSM_CSIPHY_NAME, csiphy->id);
603 
604 	ret = devm_request_irq(dev, csiphy->irq, csiphy->ops->isr,
605 			       IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,
606 			       csiphy->irq_name, csiphy);
607 	if (ret < 0) {
608 		dev_err(dev, "request_irq failed: %d\n", ret);
609 		return ret;
610 	}
611 
612 	/* Clocks */
613 
614 	csiphy->nclocks = 0;
615 	while (res->clock[csiphy->nclocks])
616 		csiphy->nclocks++;
617 
618 	csiphy->clock = devm_kcalloc(dev,
619 				     csiphy->nclocks, sizeof(*csiphy->clock),
620 				     GFP_KERNEL);
621 	if (!csiphy->clock)
622 		return -ENOMEM;
623 
624 	csiphy->rate_set = devm_kcalloc(dev,
625 					csiphy->nclocks,
626 					sizeof(*csiphy->rate_set),
627 					GFP_KERNEL);
628 	if (!csiphy->rate_set)
629 		return -ENOMEM;
630 
631 	for (i = 0; i < csiphy->nclocks; i++) {
632 		struct camss_clock *clock = &csiphy->clock[i];
633 
634 		clock->clk = devm_clk_get(dev, res->clock[i]);
635 		if (IS_ERR(clock->clk))
636 			return PTR_ERR(clock->clk);
637 
638 		clock->name = res->clock[i];
639 
640 		clock->nfreqs = 0;
641 		while (res->clock_rate[i][clock->nfreqs])
642 			clock->nfreqs++;
643 
644 		if (!clock->nfreqs) {
645 			clock->freq = NULL;
646 			continue;
647 		}
648 
649 		clock->freq = devm_kcalloc(dev,
650 					   clock->nfreqs,
651 					   sizeof(*clock->freq),
652 					   GFP_KERNEL);
653 		if (!clock->freq)
654 			return -ENOMEM;
655 
656 		for (j = 0; j < clock->nfreqs; j++)
657 			clock->freq[j] = res->clock_rate[i][j];
658 
659 		if (!strcmp(clock->name, "csiphy0_timer") ||
660 		    !strcmp(clock->name, "csiphy1_timer") ||
661 		    !strcmp(clock->name, "csiphy2_timer") ||
662 		    !strcmp(clock->name, "csiphy3_timer") ||
663 		    !strcmp(clock->name, "csiphy4_timer") ||
664 		    !strcmp(clock->name, "csiphy5_timer"))
665 			csiphy->rate_set[i] = true;
666 
667 		if (camss->version == CAMSS_660 &&
668 		    (!strcmp(clock->name, "csi0_phy") ||
669 		     !strcmp(clock->name, "csi1_phy") ||
670 		     !strcmp(clock->name, "csi2_phy")))
671 			csiphy->rate_set[i] = true;
672 	}
673 
674 	return 0;
675 }
676 
677 /*
678  * csiphy_link_setup - Setup CSIPHY connections
679  * @entity: Pointer to media entity structure
680  * @local: Pointer to local pad
681  * @remote: Pointer to remote pad
682  * @flags: Link flags
683  *
684  * Rreturn 0 on success
685  */
686 static int csiphy_link_setup(struct media_entity *entity,
687 			     const struct media_pad *local,
688 			     const struct media_pad *remote, u32 flags)
689 {
690 	if ((local->flags & MEDIA_PAD_FL_SOURCE) &&
691 	    (flags & MEDIA_LNK_FL_ENABLED)) {
692 		struct v4l2_subdev *sd;
693 		struct csiphy_device *csiphy;
694 		struct csid_device *csid;
695 
696 		if (media_pad_remote_pad_first(local))
697 			return -EBUSY;
698 
699 		sd = media_entity_to_v4l2_subdev(entity);
700 		csiphy = v4l2_get_subdevdata(sd);
701 
702 		sd = media_entity_to_v4l2_subdev(remote->entity);
703 		csid = v4l2_get_subdevdata(sd);
704 
705 		csiphy->cfg.csid_id = csid->id;
706 	}
707 
708 	return 0;
709 }
710 
711 static const struct v4l2_subdev_core_ops csiphy_core_ops = {
712 	.s_power = csiphy_set_power,
713 };
714 
715 static const struct v4l2_subdev_video_ops csiphy_video_ops = {
716 	.s_stream = csiphy_set_stream,
717 };
718 
719 static const struct v4l2_subdev_pad_ops csiphy_pad_ops = {
720 	.enum_mbus_code = csiphy_enum_mbus_code,
721 	.enum_frame_size = csiphy_enum_frame_size,
722 	.get_fmt = csiphy_get_format,
723 	.set_fmt = csiphy_set_format,
724 };
725 
726 static const struct v4l2_subdev_ops csiphy_v4l2_ops = {
727 	.core = &csiphy_core_ops,
728 	.video = &csiphy_video_ops,
729 	.pad = &csiphy_pad_ops,
730 };
731 
732 static const struct v4l2_subdev_internal_ops csiphy_v4l2_internal_ops = {
733 	.open = csiphy_init_formats,
734 };
735 
736 static const struct media_entity_operations csiphy_media_ops = {
737 	.link_setup = csiphy_link_setup,
738 	.link_validate = v4l2_subdev_link_validate,
739 };
740 
741 /*
742  * msm_csiphy_register_entity - Register subdev node for CSIPHY module
743  * @csiphy: CSIPHY device
744  * @v4l2_dev: V4L2 device
745  *
746  * Return 0 on success or a negative error code otherwise
747  */
748 int msm_csiphy_register_entity(struct csiphy_device *csiphy,
749 			       struct v4l2_device *v4l2_dev)
750 {
751 	struct v4l2_subdev *sd = &csiphy->subdev;
752 	struct media_pad *pads = csiphy->pads;
753 	struct device *dev = csiphy->camss->dev;
754 	int ret;
755 
756 	v4l2_subdev_init(sd, &csiphy_v4l2_ops);
757 	sd->internal_ops = &csiphy_v4l2_internal_ops;
758 	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
759 	snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
760 		 MSM_CSIPHY_NAME, csiphy->id);
761 	v4l2_set_subdevdata(sd, csiphy);
762 
763 	ret = csiphy_init_formats(sd, NULL);
764 	if (ret < 0) {
765 		dev_err(dev, "Failed to init format: %d\n", ret);
766 		return ret;
767 	}
768 
769 	pads[MSM_CSIPHY_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
770 	pads[MSM_CSIPHY_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
771 
772 	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
773 	sd->entity.ops = &csiphy_media_ops;
774 	ret = media_entity_pads_init(&sd->entity, MSM_CSIPHY_PADS_NUM, pads);
775 	if (ret < 0) {
776 		dev_err(dev, "Failed to init media entity: %d\n", ret);
777 		return ret;
778 	}
779 
780 	ret = v4l2_device_register_subdev(v4l2_dev, sd);
781 	if (ret < 0) {
782 		dev_err(dev, "Failed to register subdev: %d\n", ret);
783 		media_entity_cleanup(&sd->entity);
784 	}
785 
786 	return ret;
787 }
788 
789 /*
790  * msm_csiphy_unregister_entity - Unregister CSIPHY module subdev node
791  * @csiphy: CSIPHY device
792  */
793 void msm_csiphy_unregister_entity(struct csiphy_device *csiphy)
794 {
795 	v4l2_device_unregister_subdev(&csiphy->subdev);
796 	media_entity_cleanup(&csiphy->subdev.entity);
797 }
798