1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * stf_camss.c
4  *
5  * Starfive Camera Subsystem driver
6  *
7  * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
8  *
9  * Author: Jack Zhu <jack.zhu@starfivetech.com>
10  * Author: Changhuang Liang <changhuang.liang@starfivetech.com>
11  *
12  */
13 #include <linux/module.h>
14 #include <linux/of.h>
15 #include <linux/of_graph.h>
16 #include <linux/platform_device.h>
17 #include <linux/pm_runtime.h>
18 #include <linux/videodev2.h>
19 #include <media/v4l2-fwnode.h>
20 #include <media/v4l2-mc.h>
21 
22 #include "stf-camss.h"
23 
24 static const char * const stfcamss_clocks[] = {
25 	"wrapper_clk_c",
26 	"ispcore_2x",
27 	"isp_axi",
28 };
29 
30 static const char * const stfcamss_resets[] = {
31 	"wrapper_p",
32 	"wrapper_c",
33 	"axiwr",
34 	"isp_top_n",
35 	"isp_top_axi",
36 };
37 
38 static const struct stf_isr_data stf_isrs[] = {
39 	{"wr_irq", stf_wr_irq_handler},
40 	{"isp_irq", stf_isp_irq_handler},
41 	{"line_irq", stf_line_irq_handler},
42 };
43 
stfcamss_get_mem_res(struct stfcamss * stfcamss)44 static int stfcamss_get_mem_res(struct stfcamss *stfcamss)
45 {
46 	struct platform_device *pdev = to_platform_device(stfcamss->dev);
47 
48 	stfcamss->syscon_base =
49 		devm_platform_ioremap_resource_byname(pdev, "syscon");
50 	if (IS_ERR(stfcamss->syscon_base))
51 		return PTR_ERR(stfcamss->syscon_base);
52 
53 	stfcamss->isp_base = devm_platform_ioremap_resource_byname(pdev, "isp");
54 	if (IS_ERR(stfcamss->isp_base))
55 		return PTR_ERR(stfcamss->isp_base);
56 
57 	return 0;
58 }
59 
60 /*
61  * stfcamss_of_parse_endpoint_node - Parse port endpoint node
62  * @dev: Device
63  * @node: Device node to be parsed
64  * @csd: Parsed data from port endpoint node
65  *
66  * Return 0 on success or a negative error code on failure
67  */
stfcamss_of_parse_endpoint_node(struct stfcamss * stfcamss,struct device_node * node,struct stfcamss_async_subdev * csd)68 static int stfcamss_of_parse_endpoint_node(struct stfcamss *stfcamss,
69 					   struct device_node *node,
70 					   struct stfcamss_async_subdev *csd)
71 {
72 	struct v4l2_fwnode_endpoint vep = { { 0 } };
73 	int ret;
74 
75 	ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(node), &vep);
76 	if (ret) {
77 		dev_err(stfcamss->dev, "endpoint not defined at %pOF\n", node);
78 		return ret;
79 	}
80 
81 	csd->port = vep.base.port;
82 
83 	return 0;
84 }
85 
86 /*
87  * stfcamss_of_parse_ports - Parse ports node
88  * @stfcamss: STFCAMSS device
89  *
90  * Return number of "port" nodes found in "ports" node
91  */
stfcamss_of_parse_ports(struct stfcamss * stfcamss)92 static int stfcamss_of_parse_ports(struct stfcamss *stfcamss)
93 {
94 	struct device_node *node = NULL;
95 	int ret, num_subdevs = 0;
96 
97 	for_each_endpoint_of_node(stfcamss->dev->of_node, node) {
98 		struct stfcamss_async_subdev *csd;
99 
100 		if (!of_device_is_available(node))
101 			continue;
102 
103 		csd = v4l2_async_nf_add_fwnode_remote(&stfcamss->notifier,
104 						      of_fwnode_handle(node),
105 						      struct stfcamss_async_subdev);
106 		if (IS_ERR(csd)) {
107 			ret = PTR_ERR(csd);
108 			dev_err(stfcamss->dev, "failed to add async notifier\n");
109 			goto err_cleanup;
110 		}
111 
112 		ret = stfcamss_of_parse_endpoint_node(stfcamss, node, csd);
113 		if (ret)
114 			goto err_cleanup;
115 
116 		num_subdevs++;
117 	}
118 
119 	return num_subdevs;
120 
121 err_cleanup:
122 	of_node_put(node);
123 	return ret;
124 }
125 
stfcamss_register_devs(struct stfcamss * stfcamss)126 static int stfcamss_register_devs(struct stfcamss *stfcamss)
127 {
128 	struct stf_capture *cap_yuv = &stfcamss->captures[STF_CAPTURE_YUV];
129 	struct stf_isp_dev *isp_dev = &stfcamss->isp_dev;
130 	int ret;
131 
132 	ret = stf_isp_register(isp_dev, &stfcamss->v4l2_dev);
133 	if (ret < 0) {
134 		dev_err(stfcamss->dev,
135 			"failed to register stf isp%d entity: %d\n", 0, ret);
136 		return ret;
137 	}
138 
139 	ret = stf_capture_register(stfcamss, &stfcamss->v4l2_dev);
140 	if (ret < 0) {
141 		dev_err(stfcamss->dev,
142 			"failed to register capture: %d\n", ret);
143 		goto err_isp_unregister;
144 	}
145 
146 	ret = media_create_pad_link(&isp_dev->subdev.entity, STF_ISP_PAD_SRC,
147 				    &cap_yuv->video.vdev.entity, 0, 0);
148 	if (ret)
149 		goto err_cap_unregister;
150 
151 	cap_yuv->video.source_subdev = &isp_dev->subdev;
152 
153 	return ret;
154 
155 err_cap_unregister:
156 	stf_capture_unregister(stfcamss);
157 err_isp_unregister:
158 	stf_isp_unregister(&stfcamss->isp_dev);
159 
160 	return ret;
161 }
162 
stfcamss_unregister_devs(struct stfcamss * stfcamss)163 static void stfcamss_unregister_devs(struct stfcamss *stfcamss)
164 {
165 	struct stf_capture *cap_yuv = &stfcamss->captures[STF_CAPTURE_YUV];
166 	struct stf_isp_dev *isp_dev = &stfcamss->isp_dev;
167 
168 	media_entity_remove_links(&isp_dev->subdev.entity);
169 	media_entity_remove_links(&cap_yuv->video.vdev.entity);
170 
171 	stf_isp_unregister(&stfcamss->isp_dev);
172 	stf_capture_unregister(stfcamss);
173 }
174 
stfcamss_subdev_notifier_bound(struct v4l2_async_notifier * async,struct v4l2_subdev * subdev,struct v4l2_async_connection * asc)175 static int stfcamss_subdev_notifier_bound(struct v4l2_async_notifier *async,
176 					  struct v4l2_subdev *subdev,
177 					  struct v4l2_async_connection *asc)
178 {
179 	struct stfcamss *stfcamss =
180 		container_of(async, struct stfcamss, notifier);
181 	struct stfcamss_async_subdev *csd =
182 		container_of(asc, struct stfcamss_async_subdev, asd);
183 	enum stf_port_num port = csd->port;
184 	struct stf_isp_dev *isp_dev = &stfcamss->isp_dev;
185 	struct stf_capture *cap_raw = &stfcamss->captures[STF_CAPTURE_RAW];
186 	struct media_pad *pad;
187 	int ret;
188 
189 	if (port == STF_PORT_CSI2RX) {
190 		pad = &isp_dev->pads[STF_ISP_PAD_SINK];
191 	} else {
192 		dev_err(stfcamss->dev, "not support port %d\n", port);
193 		return -EPERM;
194 	}
195 
196 	ret = v4l2_create_fwnode_links_to_pad(subdev, pad, 0);
197 	if (ret)
198 		return ret;
199 
200 	ret = media_create_pad_link(&subdev->entity, 1,
201 				    &cap_raw->video.vdev.entity, 0, 0);
202 	if (ret)
203 		return ret;
204 
205 	isp_dev->source_subdev = subdev;
206 	cap_raw->video.source_subdev = subdev;
207 
208 	return 0;
209 }
210 
stfcamss_subdev_notifier_complete(struct v4l2_async_notifier * ntf)211 static int stfcamss_subdev_notifier_complete(struct v4l2_async_notifier *ntf)
212 {
213 	struct stfcamss *stfcamss =
214 		container_of(ntf, struct stfcamss, notifier);
215 
216 	return v4l2_device_register_subdev_nodes(&stfcamss->v4l2_dev);
217 }
218 
219 static const struct v4l2_async_notifier_operations
220 stfcamss_subdev_notifier_ops = {
221 	.bound = stfcamss_subdev_notifier_bound,
222 	.complete = stfcamss_subdev_notifier_complete,
223 };
224 
stfcamss_mc_init(struct platform_device * pdev,struct stfcamss * stfcamss)225 static void stfcamss_mc_init(struct platform_device *pdev,
226 			     struct stfcamss *stfcamss)
227 {
228 	stfcamss->media_dev.dev = stfcamss->dev;
229 	strscpy(stfcamss->media_dev.model, "Starfive Camera Subsystem",
230 		sizeof(stfcamss->media_dev.model));
231 	media_device_init(&stfcamss->media_dev);
232 
233 	stfcamss->v4l2_dev.mdev = &stfcamss->media_dev;
234 }
235 
236 /*
237  * stfcamss_probe - Probe STFCAMSS platform device
238  * @pdev: Pointer to STFCAMSS platform device
239  *
240  * Return 0 on success or a negative error code on failure
241  */
stfcamss_probe(struct platform_device * pdev)242 static int stfcamss_probe(struct platform_device *pdev)
243 {
244 	struct stfcamss *stfcamss;
245 	struct device *dev = &pdev->dev;
246 	int ret, num_subdevs;
247 	unsigned int i;
248 
249 	stfcamss = devm_kzalloc(dev, sizeof(*stfcamss), GFP_KERNEL);
250 	if (!stfcamss)
251 		return -ENOMEM;
252 
253 	stfcamss->dev = dev;
254 
255 	for (i = 0; i < ARRAY_SIZE(stf_isrs); ++i) {
256 		int irq;
257 
258 		irq = platform_get_irq(pdev, i);
259 		if (irq < 0)
260 			return irq;
261 
262 		ret = devm_request_irq(stfcamss->dev, irq, stf_isrs[i].isr, 0,
263 				       stf_isrs[i].name, stfcamss);
264 		if (ret) {
265 			dev_err(dev, "request irq failed: %d\n", ret);
266 			return ret;
267 		}
268 	}
269 
270 	stfcamss->nclks = ARRAY_SIZE(stfcamss->sys_clk);
271 	for (i = 0; i < stfcamss->nclks; ++i)
272 		stfcamss->sys_clk[i].id = stfcamss_clocks[i];
273 	ret = devm_clk_bulk_get(dev, stfcamss->nclks, stfcamss->sys_clk);
274 	if (ret) {
275 		dev_err(dev, "Failed to get clk controls\n");
276 		return ret;
277 	}
278 
279 	stfcamss->nrsts = ARRAY_SIZE(stfcamss->sys_rst);
280 	for (i = 0; i < stfcamss->nrsts; ++i)
281 		stfcamss->sys_rst[i].id = stfcamss_resets[i];
282 	ret = devm_reset_control_bulk_get_shared(dev, stfcamss->nrsts,
283 						 stfcamss->sys_rst);
284 	if (ret) {
285 		dev_err(dev, "Failed to get reset controls\n");
286 		return ret;
287 	}
288 
289 	ret = stfcamss_get_mem_res(stfcamss);
290 	if (ret) {
291 		dev_err(dev, "Could not map registers\n");
292 		return ret;
293 	}
294 
295 	platform_set_drvdata(pdev, stfcamss);
296 
297 	v4l2_async_nf_init(&stfcamss->notifier, &stfcamss->v4l2_dev);
298 
299 	num_subdevs = stfcamss_of_parse_ports(stfcamss);
300 	if (num_subdevs < 0) {
301 		ret = -ENODEV;
302 		dev_err(dev, "Failed to get sub devices: %d\n", ret);
303 		goto err_cleanup_notifier;
304 	}
305 
306 	ret = stf_isp_init(stfcamss);
307 	if (ret < 0) {
308 		dev_err(dev, "Failed to init isp: %d\n", ret);
309 		goto err_cleanup_notifier;
310 	}
311 
312 	stfcamss_mc_init(pdev, stfcamss);
313 
314 	ret = v4l2_device_register(stfcamss->dev, &stfcamss->v4l2_dev);
315 	if (ret < 0) {
316 		dev_err(dev, "Failed to register V4L2 device: %d\n", ret);
317 		goto err_cleanup_media_device;
318 	}
319 
320 	ret = media_device_register(&stfcamss->media_dev);
321 	if (ret) {
322 		dev_err(dev, "Failed to register media device: %d\n", ret);
323 		goto err_unregister_device;
324 	}
325 
326 	ret = stfcamss_register_devs(stfcamss);
327 	if (ret < 0) {
328 		dev_err(dev, "Failed to register subdevice: %d\n", ret);
329 		goto err_unregister_media_dev;
330 	}
331 
332 	pm_runtime_enable(dev);
333 
334 	stfcamss->notifier.ops = &stfcamss_subdev_notifier_ops;
335 	ret = v4l2_async_nf_register(&stfcamss->notifier);
336 	if (ret) {
337 		dev_err(dev, "Failed to register async subdev nodes: %d\n",
338 			ret);
339 		pm_runtime_disable(dev);
340 		goto err_unregister_subdevs;
341 	}
342 
343 	return 0;
344 
345 err_unregister_subdevs:
346 	stfcamss_unregister_devs(stfcamss);
347 err_unregister_media_dev:
348 	media_device_unregister(&stfcamss->media_dev);
349 err_unregister_device:
350 	v4l2_device_unregister(&stfcamss->v4l2_dev);
351 err_cleanup_media_device:
352 	media_device_cleanup(&stfcamss->media_dev);
353 err_cleanup_notifier:
354 	v4l2_async_nf_cleanup(&stfcamss->notifier);
355 	return ret;
356 }
357 
358 /*
359  * stfcamss_remove - Remove STFCAMSS platform device
360  * @pdev: Pointer to STFCAMSS platform device
361  */
stfcamss_remove(struct platform_device * pdev)362 static void stfcamss_remove(struct platform_device *pdev)
363 {
364 	struct stfcamss *stfcamss = platform_get_drvdata(pdev);
365 
366 	stfcamss_unregister_devs(stfcamss);
367 	v4l2_device_unregister(&stfcamss->v4l2_dev);
368 	media_device_cleanup(&stfcamss->media_dev);
369 	v4l2_async_nf_cleanup(&stfcamss->notifier);
370 	pm_runtime_disable(&pdev->dev);
371 }
372 
373 static const struct of_device_id stfcamss_of_match[] = {
374 	{ .compatible = "starfive,jh7110-camss" },
375 	{ /* sentinel */ },
376 };
377 
378 MODULE_DEVICE_TABLE(of, stfcamss_of_match);
379 
stfcamss_runtime_suspend(struct device * dev)380 static int __maybe_unused stfcamss_runtime_suspend(struct device *dev)
381 {
382 	struct stfcamss *stfcamss = dev_get_drvdata(dev);
383 	int ret;
384 
385 	ret = reset_control_bulk_assert(stfcamss->nrsts, stfcamss->sys_rst);
386 	if (ret) {
387 		dev_err(dev, "reset assert failed\n");
388 		return ret;
389 	}
390 
391 	clk_bulk_disable_unprepare(stfcamss->nclks, stfcamss->sys_clk);
392 
393 	return 0;
394 }
395 
stfcamss_runtime_resume(struct device * dev)396 static int __maybe_unused stfcamss_runtime_resume(struct device *dev)
397 {
398 	struct stfcamss *stfcamss = dev_get_drvdata(dev);
399 	int ret;
400 
401 	ret = clk_bulk_prepare_enable(stfcamss->nclks, stfcamss->sys_clk);
402 	if (ret) {
403 		dev_err(dev, "clock prepare enable failed\n");
404 		return ret;
405 	}
406 
407 	ret = reset_control_bulk_deassert(stfcamss->nrsts, stfcamss->sys_rst);
408 	if (ret < 0) {
409 		dev_err(dev, "cannot deassert resets\n");
410 		clk_bulk_disable_unprepare(stfcamss->nclks, stfcamss->sys_clk);
411 		return ret;
412 	}
413 
414 	return 0;
415 }
416 
417 static const struct dev_pm_ops stfcamss_pm_ops = {
418 	SET_RUNTIME_PM_OPS(stfcamss_runtime_suspend,
419 			   stfcamss_runtime_resume,
420 			   NULL)
421 };
422 
423 static struct platform_driver stfcamss_driver = {
424 	.probe = stfcamss_probe,
425 	.remove = stfcamss_remove,
426 	.driver = {
427 		.name = "starfive-camss",
428 		.pm = &stfcamss_pm_ops,
429 		.of_match_table = stfcamss_of_match,
430 	},
431 };
432 
433 module_platform_driver(stfcamss_driver);
434 
435 MODULE_AUTHOR("Jack Zhu <jack.zhu@starfivetech.com>");
436 MODULE_AUTHOR("Changhuang Liang <changhuang.liang@starfivetech.com>");
437 MODULE_DESCRIPTION("StarFive Camera Subsystem driver");
438 MODULE_LICENSE("GPL");
439