1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * camss.c
4  *
5  * Qualcomm MSM Camera Subsystem - Core
6  *
7  * Copyright (c) 2015, The Linux Foundation. All rights reserved.
8  * Copyright (C) 2015-2018 Linaro Ltd.
9  */
10 #include <linux/clk.h>
11 #include <linux/media-bus-format.h>
12 #include <linux/media.h>
13 #include <linux/module.h>
14 #include <linux/platform_device.h>
15 #include <linux/of.h>
16 #include <linux/of_graph.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/pm_domain.h>
19 #include <linux/slab.h>
20 #include <linux/videodev2.h>
21 
22 #include <media/media-device.h>
23 #include <media/v4l2-async.h>
24 #include <media/v4l2-device.h>
25 #include <media/v4l2-mc.h>
26 #include <media/v4l2-fwnode.h>
27 
28 #include "camss.h"
29 
30 #define CAMSS_CLOCK_MARGIN_NUMERATOR 105
31 #define CAMSS_CLOCK_MARGIN_DENOMINATOR 100
32 
33 static const struct resources csiphy_res_8x16[] = {
34 	/* CSIPHY0 */
35 	{
36 		.regulator = { NULL },
37 		.clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy0_timer" },
38 		.clock_rate = { { 0 },
39 				{ 0 },
40 				{ 0 },
41 				{ 100000000, 200000000 } },
42 		.reg = { "csiphy0", "csiphy0_clk_mux" },
43 		.interrupt = { "csiphy0" }
44 	},
45 
46 	/* CSIPHY1 */
47 	{
48 		.regulator = { NULL },
49 		.clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy1_timer" },
50 		.clock_rate = { { 0 },
51 				{ 0 },
52 				{ 0 },
53 				{ 100000000, 200000000 } },
54 		.reg = { "csiphy1", "csiphy1_clk_mux" },
55 		.interrupt = { "csiphy1" }
56 	}
57 };
58 
59 static const struct resources csid_res_8x16[] = {
60 	/* CSID0 */
61 	{
62 		.regulator = { "vdda" },
63 		.clock = { "top_ahb", "ispif_ahb", "csi0_ahb", "ahb",
64 			   "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
65 		.clock_rate = { { 0 },
66 				{ 0 },
67 				{ 0 },
68 				{ 0 },
69 				{ 100000000, 200000000 },
70 				{ 0 },
71 				{ 0 },
72 				{ 0 } },
73 		.reg = { "csid0" },
74 		.interrupt = { "csid0" }
75 	},
76 
77 	/* CSID1 */
78 	{
79 		.regulator = { "vdda" },
80 		.clock = { "top_ahb", "ispif_ahb", "csi1_ahb", "ahb",
81 			   "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
82 		.clock_rate = { { 0 },
83 				{ 0 },
84 				{ 0 },
85 				{ 0 },
86 				{ 100000000, 200000000 },
87 				{ 0 },
88 				{ 0 },
89 				{ 0 } },
90 		.reg = { "csid1" },
91 		.interrupt = { "csid1" }
92 	},
93 };
94 
95 static const struct resources_ispif ispif_res_8x16 = {
96 	/* ISPIF */
97 	.clock = { "top_ahb", "ahb", "ispif_ahb",
98 		   "csi0", "csi0_pix", "csi0_rdi",
99 		   "csi1", "csi1_pix", "csi1_rdi" },
100 	.clock_for_reset = { "vfe0", "csi_vfe0" },
101 	.reg = { "ispif", "csi_clk_mux" },
102 	.interrupt = "ispif"
103 
104 };
105 
106 static const struct resources vfe_res_8x16[] = {
107 	/* VFE0 */
108 	{
109 		.regulator = { NULL },
110 		.clock = { "top_ahb", "vfe0", "csi_vfe0",
111 			   "vfe_ahb", "vfe_axi", "ahb" },
112 		.clock_rate = { { 0 },
113 				{ 50000000, 80000000, 100000000, 160000000,
114 				  177780000, 200000000, 266670000, 320000000,
115 				  400000000, 465000000 },
116 				{ 0 },
117 				{ 0 },
118 				{ 0 },
119 				{ 0 },
120 				{ 0 },
121 				{ 0 },
122 				{ 0 } },
123 		.reg = { "vfe0" },
124 		.interrupt = { "vfe0" }
125 	}
126 };
127 
128 static const struct resources csiphy_res_8x96[] = {
129 	/* CSIPHY0 */
130 	{
131 		.regulator = { NULL },
132 		.clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy0_timer" },
133 		.clock_rate = { { 0 },
134 				{ 0 },
135 				{ 0 },
136 				{ 100000000, 200000000, 266666667 } },
137 		.reg = { "csiphy0", "csiphy0_clk_mux" },
138 		.interrupt = { "csiphy0" }
139 	},
140 
141 	/* CSIPHY1 */
142 	{
143 		.regulator = { NULL },
144 		.clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy1_timer" },
145 		.clock_rate = { { 0 },
146 				{ 0 },
147 				{ 0 },
148 				{ 100000000, 200000000, 266666667 } },
149 		.reg = { "csiphy1", "csiphy1_clk_mux" },
150 		.interrupt = { "csiphy1" }
151 	},
152 
153 	/* CSIPHY2 */
154 	{
155 		.regulator = { NULL },
156 		.clock = { "top_ahb", "ispif_ahb", "ahb", "csiphy2_timer" },
157 		.clock_rate = { { 0 },
158 				{ 0 },
159 				{ 0 },
160 				{ 100000000, 200000000, 266666667 } },
161 		.reg = { "csiphy2", "csiphy2_clk_mux" },
162 		.interrupt = { "csiphy2" }
163 	}
164 };
165 
166 static const struct resources csid_res_8x96[] = {
167 	/* CSID0 */
168 	{
169 		.regulator = { "vdda" },
170 		.clock = { "top_ahb", "ispif_ahb", "csi0_ahb", "ahb",
171 			   "csi0", "csi0_phy", "csi0_pix", "csi0_rdi" },
172 		.clock_rate = { { 0 },
173 				{ 0 },
174 				{ 0 },
175 				{ 0 },
176 				{ 100000000, 200000000, 266666667 },
177 				{ 0 },
178 				{ 0 },
179 				{ 0 } },
180 		.reg = { "csid0" },
181 		.interrupt = { "csid0" }
182 	},
183 
184 	/* CSID1 */
185 	{
186 		.regulator = { "vdda" },
187 		.clock = { "top_ahb", "ispif_ahb", "csi1_ahb", "ahb",
188 			   "csi1", "csi1_phy", "csi1_pix", "csi1_rdi" },
189 		.clock_rate = { { 0 },
190 				{ 0 },
191 				{ 0 },
192 				{ 0 },
193 				{ 100000000, 200000000, 266666667 },
194 				{ 0 },
195 				{ 0 },
196 				{ 0 } },
197 		.reg = { "csid1" },
198 		.interrupt = { "csid1" }
199 	},
200 
201 	/* CSID2 */
202 	{
203 		.regulator = { "vdda" },
204 		.clock = { "top_ahb", "ispif_ahb", "csi2_ahb", "ahb",
205 			   "csi2", "csi2_phy", "csi2_pix", "csi2_rdi" },
206 		.clock_rate = { { 0 },
207 				{ 0 },
208 				{ 0 },
209 				{ 0 },
210 				{ 100000000, 200000000, 266666667 },
211 				{ 0 },
212 				{ 0 },
213 				{ 0 } },
214 		.reg = { "csid2" },
215 		.interrupt = { "csid2" }
216 	},
217 
218 	/* CSID3 */
219 	{
220 		.regulator = { "vdda" },
221 		.clock = { "top_ahb", "ispif_ahb", "csi3_ahb", "ahb",
222 			   "csi3", "csi3_phy", "csi3_pix", "csi3_rdi" },
223 		.clock_rate = { { 0 },
224 				{ 0 },
225 				{ 0 },
226 				{ 0 },
227 				{ 100000000, 200000000, 266666667 },
228 				{ 0 },
229 				{ 0 },
230 				{ 0 } },
231 		.reg = { "csid3" },
232 		.interrupt = { "csid3" }
233 	}
234 };
235 
236 static const struct resources_ispif ispif_res_8x96 = {
237 	/* ISPIF */
238 	.clock = { "top_ahb", "ahb", "ispif_ahb",
239 		   "csi0", "csi0_pix", "csi0_rdi",
240 		   "csi1", "csi1_pix", "csi1_rdi",
241 		   "csi2", "csi2_pix", "csi2_rdi",
242 		   "csi3", "csi3_pix", "csi3_rdi" },
243 	.clock_for_reset = { "vfe0", "csi_vfe0", "vfe1", "csi_vfe1" },
244 	.reg = { "ispif", "csi_clk_mux" },
245 	.interrupt = "ispif"
246 };
247 
248 static const struct resources vfe_res_8x96[] = {
249 	/* VFE0 */
250 	{
251 		.regulator = { NULL },
252 		.clock = { "top_ahb", "ahb", "vfe0", "csi_vfe0", "vfe_ahb",
253 			   "vfe0_ahb", "vfe_axi", "vfe0_stream"},
254 		.clock_rate = { { 0 },
255 				{ 0 },
256 				{ 75000000, 100000000, 300000000,
257 				  320000000, 480000000, 600000000 },
258 				{ 0 },
259 				{ 0 },
260 				{ 0 },
261 				{ 0 },
262 				{ 0 } },
263 		.reg = { "vfe0" },
264 		.interrupt = { "vfe0" }
265 	},
266 
267 	/* VFE1 */
268 	{
269 		.regulator = { NULL },
270 		.clock = { "top_ahb", "ahb", "vfe1", "csi_vfe1", "vfe_ahb",
271 			   "vfe1_ahb", "vfe_axi", "vfe1_stream"},
272 		.clock_rate = { { 0 },
273 				{ 0 },
274 				{ 75000000, 100000000, 300000000,
275 				  320000000, 480000000, 600000000 },
276 				{ 0 },
277 				{ 0 },
278 				{ 0 },
279 				{ 0 },
280 				{ 0 } },
281 		.reg = { "vfe1" },
282 		.interrupt = { "vfe1" }
283 	}
284 };
285 
286 /*
287  * camss_add_clock_margin - Add margin to clock frequency rate
288  * @rate: Clock frequency rate
289  *
290  * When making calculations with physical clock frequency values
291  * some safety margin must be added. Add it.
292  */
293 inline void camss_add_clock_margin(u64 *rate)
294 {
295 	*rate *= CAMSS_CLOCK_MARGIN_NUMERATOR;
296 	*rate = div_u64(*rate, CAMSS_CLOCK_MARGIN_DENOMINATOR);
297 }
298 
299 /*
300  * camss_enable_clocks - Enable multiple clocks
301  * @nclocks: Number of clocks in clock array
302  * @clock: Clock array
303  * @dev: Device
304  *
305  * Return 0 on success or a negative error code otherwise
306  */
307 int camss_enable_clocks(int nclocks, struct camss_clock *clock,
308 			struct device *dev)
309 {
310 	int ret;
311 	int i;
312 
313 	for (i = 0; i < nclocks; i++) {
314 		ret = clk_prepare_enable(clock[i].clk);
315 		if (ret) {
316 			dev_err(dev, "clock enable failed: %d\n", ret);
317 			goto error;
318 		}
319 	}
320 
321 	return 0;
322 
323 error:
324 	for (i--; i >= 0; i--)
325 		clk_disable_unprepare(clock[i].clk);
326 
327 	return ret;
328 }
329 
330 /*
331  * camss_disable_clocks - Disable multiple clocks
332  * @nclocks: Number of clocks in clock array
333  * @clock: Clock array
334  */
335 void camss_disable_clocks(int nclocks, struct camss_clock *clock)
336 {
337 	int i;
338 
339 	for (i = nclocks - 1; i >= 0; i--)
340 		clk_disable_unprepare(clock[i].clk);
341 }
342 
343 /*
344  * camss_find_sensor - Find a linked media entity which represents a sensor
345  * @entity: Media entity to start searching from
346  *
347  * Return a pointer to sensor media entity or NULL if not found
348  */
349 struct media_entity *camss_find_sensor(struct media_entity *entity)
350 {
351 	struct media_pad *pad;
352 
353 	while (1) {
354 		pad = &entity->pads[0];
355 		if (!(pad->flags & MEDIA_PAD_FL_SINK))
356 			return NULL;
357 
358 		pad = media_entity_remote_pad(pad);
359 		if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
360 			return NULL;
361 
362 		entity = pad->entity;
363 
364 		if (entity->function == MEDIA_ENT_F_CAM_SENSOR)
365 			return entity;
366 	}
367 }
368 
369 /*
370  * camss_get_pixel_clock - Get pixel clock rate from sensor
371  * @entity: Media entity in the current pipeline
372  * @pixel_clock: Received pixel clock value
373  *
374  * Return 0 on success or a negative error code otherwise
375  */
376 int camss_get_pixel_clock(struct media_entity *entity, u32 *pixel_clock)
377 {
378 	struct media_entity *sensor;
379 	struct v4l2_subdev *subdev;
380 	struct v4l2_ctrl *ctrl;
381 
382 	sensor = camss_find_sensor(entity);
383 	if (!sensor)
384 		return -ENODEV;
385 
386 	subdev = media_entity_to_v4l2_subdev(sensor);
387 
388 	ctrl = v4l2_ctrl_find(subdev->ctrl_handler, V4L2_CID_PIXEL_RATE);
389 
390 	if (!ctrl)
391 		return -EINVAL;
392 
393 	*pixel_clock = v4l2_ctrl_g_ctrl_int64(ctrl);
394 
395 	return 0;
396 }
397 
398 int camss_pm_domain_on(struct camss *camss, int id)
399 {
400 	if (camss->version == CAMSS_8x96) {
401 		camss->genpd_link[id] = device_link_add(camss->dev,
402 				camss->genpd[id], DL_FLAG_STATELESS |
403 				DL_FLAG_PM_RUNTIME | DL_FLAG_RPM_ACTIVE);
404 
405 		if (!camss->genpd_link[id])
406 			return -EINVAL;
407 	}
408 
409 	return 0;
410 }
411 
412 void camss_pm_domain_off(struct camss *camss, int id)
413 {
414 	if (camss->version == CAMSS_8x96)
415 		device_link_del(camss->genpd_link[id]);
416 }
417 
418 /*
419  * camss_of_parse_endpoint_node - Parse port endpoint node
420  * @dev: Device
421  * @node: Device node to be parsed
422  * @csd: Parsed data from port endpoint node
423  *
424  * Return 0 on success or a negative error code on failure
425  */
426 static int camss_of_parse_endpoint_node(struct device *dev,
427 					struct device_node *node,
428 					struct camss_async_subdev *csd)
429 {
430 	struct csiphy_lanes_cfg *lncfg = &csd->interface.csi2.lane_cfg;
431 	struct v4l2_fwnode_bus_mipi_csi2 *mipi_csi2;
432 	struct v4l2_fwnode_endpoint vep = { { 0 } };
433 	unsigned int i;
434 
435 	v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
436 
437 	csd->interface.csiphy_id = vep.base.port;
438 
439 	mipi_csi2 = &vep.bus.mipi_csi2;
440 	lncfg->clk.pos = mipi_csi2->clock_lane;
441 	lncfg->clk.pol = mipi_csi2->lane_polarities[0];
442 	lncfg->num_data = mipi_csi2->num_data_lanes;
443 
444 	lncfg->data = devm_kcalloc(dev,
445 				   lncfg->num_data, sizeof(*lncfg->data),
446 				   GFP_KERNEL);
447 	if (!lncfg->data)
448 		return -ENOMEM;
449 
450 	for (i = 0; i < lncfg->num_data; i++) {
451 		lncfg->data[i].pos = mipi_csi2->data_lanes[i];
452 		lncfg->data[i].pol = mipi_csi2->lane_polarities[i + 1];
453 	}
454 
455 	return 0;
456 }
457 
458 /*
459  * camss_of_parse_ports - Parse ports node
460  * @dev: Device
461  * @notifier: v4l2_device notifier data
462  *
463  * Return number of "port" nodes found in "ports" node
464  */
465 static int camss_of_parse_ports(struct camss *camss)
466 {
467 	struct device *dev = camss->dev;
468 	struct device_node *node = NULL;
469 	struct device_node *remote = NULL;
470 	int ret, num_subdevs = 0;
471 
472 	for_each_endpoint_of_node(dev->of_node, node) {
473 		struct camss_async_subdev *csd;
474 		struct v4l2_async_subdev *asd;
475 
476 		if (!of_device_is_available(node))
477 			continue;
478 
479 		remote = of_graph_get_remote_port_parent(node);
480 		if (!remote) {
481 			dev_err(dev, "Cannot get remote parent\n");
482 			ret = -EINVAL;
483 			goto err_cleanup;
484 		}
485 
486 		asd = v4l2_async_notifier_add_fwnode_subdev(
487 			&camss->notifier, of_fwnode_handle(remote),
488 			sizeof(*csd));
489 		of_node_put(remote);
490 		if (IS_ERR(asd)) {
491 			ret = PTR_ERR(asd);
492 			goto err_cleanup;
493 		}
494 
495 		csd = container_of(asd, struct camss_async_subdev, asd);
496 
497 		ret = camss_of_parse_endpoint_node(dev, node, csd);
498 		if (ret < 0)
499 			goto err_cleanup;
500 
501 		num_subdevs++;
502 	}
503 
504 	return num_subdevs;
505 
506 err_cleanup:
507 	of_node_put(node);
508 	return ret;
509 }
510 
511 /*
512  * camss_init_subdevices - Initialize subdev structures and resources
513  * @camss: CAMSS device
514  *
515  * Return 0 on success or a negative error code on failure
516  */
517 static int camss_init_subdevices(struct camss *camss)
518 {
519 	const struct resources *csiphy_res;
520 	const struct resources *csid_res;
521 	const struct resources_ispif *ispif_res;
522 	const struct resources *vfe_res;
523 	unsigned int i;
524 	int ret;
525 
526 	if (camss->version == CAMSS_8x16) {
527 		csiphy_res = csiphy_res_8x16;
528 		csid_res = csid_res_8x16;
529 		ispif_res = &ispif_res_8x16;
530 		vfe_res = vfe_res_8x16;
531 	} else if (camss->version == CAMSS_8x96) {
532 		csiphy_res = csiphy_res_8x96;
533 		csid_res = csid_res_8x96;
534 		ispif_res = &ispif_res_8x96;
535 		vfe_res = vfe_res_8x96;
536 	} else {
537 		return -EINVAL;
538 	}
539 
540 	for (i = 0; i < camss->csiphy_num; i++) {
541 		ret = msm_csiphy_subdev_init(camss, &camss->csiphy[i],
542 					     &csiphy_res[i], i);
543 		if (ret < 0) {
544 			dev_err(camss->dev,
545 				"Failed to init csiphy%d sub-device: %d\n",
546 				i, ret);
547 			return ret;
548 		}
549 	}
550 
551 	for (i = 0; i < camss->csid_num; i++) {
552 		ret = msm_csid_subdev_init(camss, &camss->csid[i],
553 					   &csid_res[i], i);
554 		if (ret < 0) {
555 			dev_err(camss->dev,
556 				"Failed to init csid%d sub-device: %d\n",
557 				i, ret);
558 			return ret;
559 		}
560 	}
561 
562 	ret = msm_ispif_subdev_init(&camss->ispif, ispif_res);
563 	if (ret < 0) {
564 		dev_err(camss->dev, "Failed to init ispif sub-device: %d\n",
565 			ret);
566 		return ret;
567 	}
568 
569 	for (i = 0; i < camss->vfe_num; i++) {
570 		ret = msm_vfe_subdev_init(camss, &camss->vfe[i],
571 					  &vfe_res[i], i);
572 		if (ret < 0) {
573 			dev_err(camss->dev,
574 				"Fail to init vfe%d sub-device: %d\n", i, ret);
575 			return ret;
576 		}
577 	}
578 
579 	return 0;
580 }
581 
582 /*
583  * camss_register_entities - Register subdev nodes and create links
584  * @camss: CAMSS device
585  *
586  * Return 0 on success or a negative error code on failure
587  */
588 static int camss_register_entities(struct camss *camss)
589 {
590 	int i, j, k;
591 	int ret;
592 
593 	for (i = 0; i < camss->csiphy_num; i++) {
594 		ret = msm_csiphy_register_entity(&camss->csiphy[i],
595 						 &camss->v4l2_dev);
596 		if (ret < 0) {
597 			dev_err(camss->dev,
598 				"Failed to register csiphy%d entity: %d\n",
599 				i, ret);
600 			goto err_reg_csiphy;
601 		}
602 	}
603 
604 	for (i = 0; i < camss->csid_num; i++) {
605 		ret = msm_csid_register_entity(&camss->csid[i],
606 					       &camss->v4l2_dev);
607 		if (ret < 0) {
608 			dev_err(camss->dev,
609 				"Failed to register csid%d entity: %d\n",
610 				i, ret);
611 			goto err_reg_csid;
612 		}
613 	}
614 
615 	ret = msm_ispif_register_entities(&camss->ispif, &camss->v4l2_dev);
616 	if (ret < 0) {
617 		dev_err(camss->dev, "Failed to register ispif entities: %d\n",
618 			ret);
619 		goto err_reg_ispif;
620 	}
621 
622 	for (i = 0; i < camss->vfe_num; i++) {
623 		ret = msm_vfe_register_entities(&camss->vfe[i],
624 						&camss->v4l2_dev);
625 		if (ret < 0) {
626 			dev_err(camss->dev,
627 				"Failed to register vfe%d entities: %d\n",
628 				i, ret);
629 			goto err_reg_vfe;
630 		}
631 	}
632 
633 	for (i = 0; i < camss->csiphy_num; i++) {
634 		for (j = 0; j < camss->csid_num; j++) {
635 			ret = media_create_pad_link(
636 				&camss->csiphy[i].subdev.entity,
637 				MSM_CSIPHY_PAD_SRC,
638 				&camss->csid[j].subdev.entity,
639 				MSM_CSID_PAD_SINK,
640 				0);
641 			if (ret < 0) {
642 				dev_err(camss->dev,
643 					"Failed to link %s->%s entities: %d\n",
644 					camss->csiphy[i].subdev.entity.name,
645 					camss->csid[j].subdev.entity.name,
646 					ret);
647 				goto err_link;
648 			}
649 		}
650 	}
651 
652 	for (i = 0; i < camss->csid_num; i++) {
653 		for (j = 0; j < camss->ispif.line_num; j++) {
654 			ret = media_create_pad_link(
655 				&camss->csid[i].subdev.entity,
656 				MSM_CSID_PAD_SRC,
657 				&camss->ispif.line[j].subdev.entity,
658 				MSM_ISPIF_PAD_SINK,
659 				0);
660 			if (ret < 0) {
661 				dev_err(camss->dev,
662 					"Failed to link %s->%s entities: %d\n",
663 					camss->csid[i].subdev.entity.name,
664 					camss->ispif.line[j].subdev.entity.name,
665 					ret);
666 				goto err_link;
667 			}
668 		}
669 	}
670 
671 	for (i = 0; i < camss->ispif.line_num; i++)
672 		for (k = 0; k < camss->vfe_num; k++)
673 			for (j = 0; j < ARRAY_SIZE(camss->vfe[k].line); j++) {
674 				ret = media_create_pad_link(
675 					&camss->ispif.line[i].subdev.entity,
676 					MSM_ISPIF_PAD_SRC,
677 					&camss->vfe[k].line[j].subdev.entity,
678 					MSM_VFE_PAD_SINK,
679 					0);
680 				if (ret < 0) {
681 					dev_err(camss->dev,
682 						"Failed to link %s->%s entities: %d\n",
683 						camss->ispif.line[i].subdev.entity.name,
684 						camss->vfe[k].line[j].subdev.entity.name,
685 						ret);
686 					goto err_link;
687 				}
688 			}
689 
690 	return 0;
691 
692 err_link:
693 	i = camss->vfe_num;
694 err_reg_vfe:
695 	for (i--; i >= 0; i--)
696 		msm_vfe_unregister_entities(&camss->vfe[i]);
697 
698 	msm_ispif_unregister_entities(&camss->ispif);
699 err_reg_ispif:
700 
701 	i = camss->csid_num;
702 err_reg_csid:
703 	for (i--; i >= 0; i--)
704 		msm_csid_unregister_entity(&camss->csid[i]);
705 
706 	i = camss->csiphy_num;
707 err_reg_csiphy:
708 	for (i--; i >= 0; i--)
709 		msm_csiphy_unregister_entity(&camss->csiphy[i]);
710 
711 	return ret;
712 }
713 
714 /*
715  * camss_unregister_entities - Unregister subdev nodes
716  * @camss: CAMSS device
717  *
718  * Return 0 on success or a negative error code on failure
719  */
720 static void camss_unregister_entities(struct camss *camss)
721 {
722 	unsigned int i;
723 
724 	for (i = 0; i < camss->csiphy_num; i++)
725 		msm_csiphy_unregister_entity(&camss->csiphy[i]);
726 
727 	for (i = 0; i < camss->csid_num; i++)
728 		msm_csid_unregister_entity(&camss->csid[i]);
729 
730 	msm_ispif_unregister_entities(&camss->ispif);
731 
732 	for (i = 0; i < camss->vfe_num; i++)
733 		msm_vfe_unregister_entities(&camss->vfe[i]);
734 }
735 
736 static int camss_subdev_notifier_bound(struct v4l2_async_notifier *async,
737 				       struct v4l2_subdev *subdev,
738 				       struct v4l2_async_subdev *asd)
739 {
740 	struct camss *camss = container_of(async, struct camss, notifier);
741 	struct camss_async_subdev *csd =
742 		container_of(asd, struct camss_async_subdev, asd);
743 	u8 id = csd->interface.csiphy_id;
744 	struct csiphy_device *csiphy = &camss->csiphy[id];
745 
746 	csiphy->cfg.csi2 = &csd->interface.csi2;
747 	subdev->host_priv = csiphy;
748 
749 	return 0;
750 }
751 
752 static int camss_subdev_notifier_complete(struct v4l2_async_notifier *async)
753 {
754 	struct camss *camss = container_of(async, struct camss, notifier);
755 	struct v4l2_device *v4l2_dev = &camss->v4l2_dev;
756 	struct v4l2_subdev *sd;
757 	int ret;
758 
759 	list_for_each_entry(sd, &v4l2_dev->subdevs, list) {
760 		if (sd->host_priv) {
761 			struct media_entity *sensor = &sd->entity;
762 			struct csiphy_device *csiphy =
763 					(struct csiphy_device *) sd->host_priv;
764 			struct media_entity *input = &csiphy->subdev.entity;
765 			unsigned int i;
766 
767 			for (i = 0; i < sensor->num_pads; i++) {
768 				if (sensor->pads[i].flags & MEDIA_PAD_FL_SOURCE)
769 					break;
770 			}
771 			if (i == sensor->num_pads) {
772 				dev_err(camss->dev,
773 					"No source pad in external entity\n");
774 				return -EINVAL;
775 			}
776 
777 			ret = media_create_pad_link(sensor, i,
778 				input, MSM_CSIPHY_PAD_SINK,
779 				MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
780 			if (ret < 0) {
781 				dev_err(camss->dev,
782 					"Failed to link %s->%s entities: %d\n",
783 					sensor->name, input->name, ret);
784 				return ret;
785 			}
786 		}
787 	}
788 
789 	ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
790 	if (ret < 0)
791 		return ret;
792 
793 	return media_device_register(&camss->media_dev);
794 }
795 
796 static const struct v4l2_async_notifier_operations camss_subdev_notifier_ops = {
797 	.bound = camss_subdev_notifier_bound,
798 	.complete = camss_subdev_notifier_complete,
799 };
800 
801 static const struct media_device_ops camss_media_ops = {
802 	.link_notify = v4l2_pipeline_link_notify,
803 };
804 
805 /*
806  * camss_probe - Probe CAMSS platform device
807  * @pdev: Pointer to CAMSS platform device
808  *
809  * Return 0 on success or a negative error code on failure
810  */
811 static int camss_probe(struct platform_device *pdev)
812 {
813 	struct device *dev = &pdev->dev;
814 	struct camss *camss;
815 	int num_subdevs, ret;
816 
817 	camss = kzalloc(sizeof(*camss), GFP_KERNEL);
818 	if (!camss)
819 		return -ENOMEM;
820 
821 	atomic_set(&camss->ref_count, 0);
822 	camss->dev = dev;
823 	platform_set_drvdata(pdev, camss);
824 
825 	if (of_device_is_compatible(dev->of_node, "qcom,msm8916-camss")) {
826 		camss->version = CAMSS_8x16;
827 		camss->csiphy_num = 2;
828 		camss->csid_num = 2;
829 		camss->vfe_num = 1;
830 	} else if (of_device_is_compatible(dev->of_node,
831 					   "qcom,msm8996-camss")) {
832 		camss->version = CAMSS_8x96;
833 		camss->csiphy_num = 3;
834 		camss->csid_num = 4;
835 		camss->vfe_num = 2;
836 	} else {
837 		ret = -EINVAL;
838 		goto err_free;
839 	}
840 
841 	camss->csiphy = devm_kcalloc(dev, camss->csiphy_num,
842 				     sizeof(*camss->csiphy), GFP_KERNEL);
843 	if (!camss->csiphy) {
844 		ret = -ENOMEM;
845 		goto err_free;
846 	}
847 
848 	camss->csid = devm_kcalloc(dev, camss->csid_num, sizeof(*camss->csid),
849 				   GFP_KERNEL);
850 	if (!camss->csid) {
851 		ret = -ENOMEM;
852 		goto err_free;
853 	}
854 
855 	camss->vfe = devm_kcalloc(dev, camss->vfe_num, sizeof(*camss->vfe),
856 				  GFP_KERNEL);
857 	if (!camss->vfe) {
858 		ret = -ENOMEM;
859 		goto err_free;
860 	}
861 
862 	v4l2_async_notifier_init(&camss->notifier);
863 
864 	num_subdevs = camss_of_parse_ports(camss);
865 	if (num_subdevs < 0) {
866 		ret = num_subdevs;
867 		goto err_cleanup;
868 	}
869 
870 	ret = camss_init_subdevices(camss);
871 	if (ret < 0)
872 		goto err_cleanup;
873 
874 	ret = dma_set_mask_and_coherent(dev, 0xffffffff);
875 	if (ret)
876 		goto err_cleanup;
877 
878 	camss->media_dev.dev = camss->dev;
879 	strscpy(camss->media_dev.model, "Qualcomm Camera Subsystem",
880 		sizeof(camss->media_dev.model));
881 	camss->media_dev.ops = &camss_media_ops;
882 	media_device_init(&camss->media_dev);
883 
884 	camss->v4l2_dev.mdev = &camss->media_dev;
885 	ret = v4l2_device_register(camss->dev, &camss->v4l2_dev);
886 	if (ret < 0) {
887 		dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
888 		goto err_cleanup;
889 	}
890 
891 	ret = camss_register_entities(camss);
892 	if (ret < 0)
893 		goto err_register_entities;
894 
895 	if (num_subdevs) {
896 		camss->notifier.ops = &camss_subdev_notifier_ops;
897 
898 		ret = v4l2_async_notifier_register(&camss->v4l2_dev,
899 						   &camss->notifier);
900 		if (ret) {
901 			dev_err(dev,
902 				"Failed to register async subdev nodes: %d\n",
903 				ret);
904 			goto err_register_subdevs;
905 		}
906 	} else {
907 		ret = v4l2_device_register_subdev_nodes(&camss->v4l2_dev);
908 		if (ret < 0) {
909 			dev_err(dev, "Failed to register subdev nodes: %d\n",
910 				ret);
911 			goto err_register_subdevs;
912 		}
913 
914 		ret = media_device_register(&camss->media_dev);
915 		if (ret < 0) {
916 			dev_err(dev, "Failed to register media device: %d\n",
917 				ret);
918 			goto err_register_subdevs;
919 		}
920 	}
921 
922 	if (camss->version == CAMSS_8x96) {
923 		camss->genpd[PM_DOMAIN_VFE0] = dev_pm_domain_attach_by_id(
924 						camss->dev, PM_DOMAIN_VFE0);
925 		if (IS_ERR(camss->genpd[PM_DOMAIN_VFE0]))
926 			return PTR_ERR(camss->genpd[PM_DOMAIN_VFE0]);
927 
928 		camss->genpd[PM_DOMAIN_VFE1] = dev_pm_domain_attach_by_id(
929 						camss->dev, PM_DOMAIN_VFE1);
930 		if (IS_ERR(camss->genpd[PM_DOMAIN_VFE1])) {
931 			dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE0],
932 					     true);
933 			return PTR_ERR(camss->genpd[PM_DOMAIN_VFE1]);
934 		}
935 	}
936 
937 	pm_runtime_enable(dev);
938 
939 	return 0;
940 
941 err_register_subdevs:
942 	camss_unregister_entities(camss);
943 err_register_entities:
944 	v4l2_device_unregister(&camss->v4l2_dev);
945 err_cleanup:
946 	v4l2_async_notifier_cleanup(&camss->notifier);
947 err_free:
948 	kfree(camss);
949 
950 	return ret;
951 }
952 
953 void camss_delete(struct camss *camss)
954 {
955 	v4l2_device_unregister(&camss->v4l2_dev);
956 	media_device_unregister(&camss->media_dev);
957 	media_device_cleanup(&camss->media_dev);
958 
959 	pm_runtime_disable(camss->dev);
960 
961 	if (camss->version == CAMSS_8x96) {
962 		dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE0], true);
963 		dev_pm_domain_detach(camss->genpd[PM_DOMAIN_VFE1], true);
964 	}
965 
966 	kfree(camss);
967 }
968 
969 /*
970  * camss_remove - Remove CAMSS platform device
971  * @pdev: Pointer to CAMSS platform device
972  *
973  * Always returns 0.
974  */
975 static int camss_remove(struct platform_device *pdev)
976 {
977 	unsigned int i;
978 
979 	struct camss *camss = platform_get_drvdata(pdev);
980 
981 	for (i = 0; i < camss->vfe_num; i++)
982 		msm_vfe_stop_streaming(&camss->vfe[i]);
983 
984 	v4l2_async_notifier_unregister(&camss->notifier);
985 	v4l2_async_notifier_cleanup(&camss->notifier);
986 	camss_unregister_entities(camss);
987 
988 	if (atomic_read(&camss->ref_count) == 0)
989 		camss_delete(camss);
990 
991 	return 0;
992 }
993 
994 static const struct of_device_id camss_dt_match[] = {
995 	{ .compatible = "qcom,msm8916-camss" },
996 	{ .compatible = "qcom,msm8996-camss" },
997 	{ }
998 };
999 
1000 MODULE_DEVICE_TABLE(of, camss_dt_match);
1001 
1002 static int __maybe_unused camss_runtime_suspend(struct device *dev)
1003 {
1004 	return 0;
1005 }
1006 
1007 static int __maybe_unused camss_runtime_resume(struct device *dev)
1008 {
1009 	return 0;
1010 }
1011 
1012 static const struct dev_pm_ops camss_pm_ops = {
1013 	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
1014 				pm_runtime_force_resume)
1015 	SET_RUNTIME_PM_OPS(camss_runtime_suspend, camss_runtime_resume, NULL)
1016 };
1017 
1018 static struct platform_driver qcom_camss_driver = {
1019 	.probe = camss_probe,
1020 	.remove = camss_remove,
1021 	.driver = {
1022 		.name = "qcom-camss",
1023 		.of_match_table = camss_dt_match,
1024 		.pm = &camss_pm_ops,
1025 	},
1026 };
1027 
1028 module_platform_driver(qcom_camss_driver);
1029 
1030 MODULE_ALIAS("platform:qcom-camss");
1031 MODULE_DESCRIPTION("Qualcomm Camera Subsystem driver");
1032 MODULE_AUTHOR("Todor Tomov <todor.tomov@linaro.org>");
1033 MODULE_LICENSE("GPL v2");
1034