1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * Driver for Cadence MIPI-CSI2 RX Controller v1.3 |
4 | * |
5 | * Copyright (C) 2017 Cadence Design Systems Inc. |
6 | */ |
7 | |
8 | #include <linux/clk.h> |
9 | #include <linux/delay.h> |
10 | #include <linux/io.h> |
11 | #include <linux/iopoll.h> |
12 | #include <linux/module.h> |
13 | #include <linux/of.h> |
14 | #include <linux/of_graph.h> |
15 | #include <linux/phy/phy.h> |
16 | #include <linux/platform_device.h> |
17 | #include <linux/reset.h> |
18 | #include <linux/slab.h> |
19 | |
20 | #include <media/v4l2-ctrls.h> |
21 | #include <media/v4l2-device.h> |
22 | #include <media/v4l2-fwnode.h> |
23 | #include <media/v4l2-subdev.h> |
24 | |
25 | #define CSI2RX_DEVICE_CFG_REG 0x000 |
26 | |
27 | #define CSI2RX_SOFT_RESET_REG 0x004 |
28 | #define CSI2RX_SOFT_RESET_PROTOCOL BIT(1) |
29 | #define CSI2RX_SOFT_RESET_FRONT BIT(0) |
30 | |
31 | #define CSI2RX_STATIC_CFG_REG 0x008 |
32 | #define CSI2RX_STATIC_CFG_DLANE_MAP(llane, plane) ((plane) << (16 + (llane) * 4)) |
33 | #define CSI2RX_STATIC_CFG_LANES_MASK GENMASK(11, 8) |
34 | |
35 | #define CSI2RX_DPHY_LANE_CTRL_REG 0x40 |
36 | #define CSI2RX_DPHY_CL_RST BIT(16) |
37 | #define CSI2RX_DPHY_DL_RST(i) BIT((i) + 12) |
38 | #define CSI2RX_DPHY_CL_EN BIT(4) |
39 | #define CSI2RX_DPHY_DL_EN(i) BIT(i) |
40 | |
41 | #define CSI2RX_STREAM_BASE(n) (((n) + 1) * 0x100) |
42 | |
43 | #define CSI2RX_STREAM_CTRL_REG(n) (CSI2RX_STREAM_BASE(n) + 0x000) |
44 | #define CSI2RX_STREAM_CTRL_SOFT_RST BIT(4) |
45 | #define CSI2RX_STREAM_CTRL_STOP BIT(1) |
46 | #define CSI2RX_STREAM_CTRL_START BIT(0) |
47 | |
48 | #define CSI2RX_STREAM_STATUS_REG(n) (CSI2RX_STREAM_BASE(n) + 0x004) |
49 | #define CSI2RX_STREAM_STATUS_RDY BIT(31) |
50 | |
51 | #define CSI2RX_STREAM_DATA_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x008) |
52 | #define CSI2RX_STREAM_DATA_CFG_VC_SELECT(n) BIT((n) + 16) |
53 | |
54 | #define CSI2RX_STREAM_CFG_REG(n) (CSI2RX_STREAM_BASE(n) + 0x00c) |
55 | #define CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF (1 << 8) |
56 | |
57 | #define CSI2RX_LANES_MAX 4 |
58 | #define CSI2RX_STREAMS_MAX 4 |
59 | |
60 | enum csi2rx_pads { |
61 | CSI2RX_PAD_SINK, |
62 | CSI2RX_PAD_SOURCE_STREAM0, |
63 | CSI2RX_PAD_SOURCE_STREAM1, |
64 | CSI2RX_PAD_SOURCE_STREAM2, |
65 | CSI2RX_PAD_SOURCE_STREAM3, |
66 | CSI2RX_PAD_MAX, |
67 | }; |
68 | |
69 | struct csi2rx_fmt { |
70 | u32 code; |
71 | u8 bpp; |
72 | }; |
73 | |
74 | struct csi2rx_priv { |
75 | struct device *dev; |
76 | unsigned int count; |
77 | |
78 | /* |
79 | * Used to prevent race conditions between multiple, |
80 | * concurrent calls to start and stop. |
81 | */ |
82 | struct mutex lock; |
83 | |
84 | void __iomem *base; |
85 | struct clk *sys_clk; |
86 | struct clk *p_clk; |
87 | struct clk *pixel_clk[CSI2RX_STREAMS_MAX]; |
88 | struct reset_control *sys_rst; |
89 | struct reset_control *p_rst; |
90 | struct reset_control *pixel_rst[CSI2RX_STREAMS_MAX]; |
91 | struct phy *dphy; |
92 | |
93 | u8 lanes[CSI2RX_LANES_MAX]; |
94 | u8 num_lanes; |
95 | u8 max_lanes; |
96 | u8 max_streams; |
97 | bool has_internal_dphy; |
98 | |
99 | struct v4l2_subdev subdev; |
100 | struct v4l2_async_notifier notifier; |
101 | struct media_pad pads[CSI2RX_PAD_MAX]; |
102 | |
103 | /* Remote source */ |
104 | struct v4l2_subdev *source_subdev; |
105 | int source_pad; |
106 | }; |
107 | |
108 | static const struct csi2rx_fmt formats[] = { |
109 | { .code = MEDIA_BUS_FMT_YUYV8_1X16, .bpp = 16, }, |
110 | { .code = MEDIA_BUS_FMT_UYVY8_1X16, .bpp = 16, }, |
111 | { .code = MEDIA_BUS_FMT_YVYU8_1X16, .bpp = 16, }, |
112 | { .code = MEDIA_BUS_FMT_VYUY8_1X16, .bpp = 16, }, |
113 | { .code = MEDIA_BUS_FMT_SBGGR8_1X8, .bpp = 8, }, |
114 | { .code = MEDIA_BUS_FMT_SGBRG8_1X8, .bpp = 8, }, |
115 | { .code = MEDIA_BUS_FMT_SGRBG8_1X8, .bpp = 8, }, |
116 | { .code = MEDIA_BUS_FMT_SRGGB8_1X8, .bpp = 8, }, |
117 | { .code = MEDIA_BUS_FMT_Y8_1X8, .bpp = 8, }, |
118 | { .code = MEDIA_BUS_FMT_SBGGR10_1X10, .bpp = 10, }, |
119 | { .code = MEDIA_BUS_FMT_SGBRG10_1X10, .bpp = 10, }, |
120 | { .code = MEDIA_BUS_FMT_SGRBG10_1X10, .bpp = 10, }, |
121 | { .code = MEDIA_BUS_FMT_SRGGB10_1X10, .bpp = 10, }, |
122 | { .code = MEDIA_BUS_FMT_RGB565_1X16, .bpp = 16, }, |
123 | { .code = MEDIA_BUS_FMT_RGB888_1X24, .bpp = 24, }, |
124 | { .code = MEDIA_BUS_FMT_BGR888_1X24, .bpp = 24, }, |
125 | }; |
126 | |
127 | static const struct csi2rx_fmt *csi2rx_get_fmt_by_code(u32 code) |
128 | { |
129 | unsigned int i; |
130 | |
131 | for (i = 0; i < ARRAY_SIZE(formats); i++) |
132 | if (formats[i].code == code) |
133 | return &formats[i]; |
134 | |
135 | return NULL; |
136 | } |
137 | |
138 | static inline |
139 | struct csi2rx_priv *v4l2_subdev_to_csi2rx(struct v4l2_subdev *subdev) |
140 | { |
141 | return container_of(subdev, struct csi2rx_priv, subdev); |
142 | } |
143 | |
144 | static void csi2rx_reset(struct csi2rx_priv *csi2rx) |
145 | { |
146 | unsigned int i; |
147 | |
148 | /* Reset module */ |
149 | writel(CSI2RX_SOFT_RESET_PROTOCOL | CSI2RX_SOFT_RESET_FRONT, |
150 | addr: csi2rx->base + CSI2RX_SOFT_RESET_REG); |
151 | /* Reset individual streams. */ |
152 | for (i = 0; i < csi2rx->max_streams; i++) { |
153 | writel(CSI2RX_STREAM_CTRL_SOFT_RST, |
154 | addr: csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); |
155 | } |
156 | |
157 | usleep_range(min: 10, max: 20); |
158 | |
159 | /* Clear resets */ |
160 | writel(val: 0, addr: csi2rx->base + CSI2RX_SOFT_RESET_REG); |
161 | for (i = 0; i < csi2rx->max_streams; i++) |
162 | writel(val: 0, addr: csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); |
163 | } |
164 | |
165 | static int csi2rx_configure_ext_dphy(struct csi2rx_priv *csi2rx) |
166 | { |
167 | union phy_configure_opts opts = { }; |
168 | struct phy_configure_opts_mipi_dphy *cfg = &opts.mipi_dphy; |
169 | struct v4l2_subdev_format sd_fmt = { |
170 | .which = V4L2_SUBDEV_FORMAT_ACTIVE, |
171 | .pad = CSI2RX_PAD_SINK, |
172 | }; |
173 | const struct csi2rx_fmt *fmt; |
174 | s64 link_freq; |
175 | int ret; |
176 | |
177 | ret = v4l2_subdev_call_state_active(&csi2rx->subdev, pad, get_fmt, |
178 | &sd_fmt); |
179 | if (ret < 0) |
180 | return ret; |
181 | |
182 | fmt = csi2rx_get_fmt_by_code(code: sd_fmt.format.code); |
183 | |
184 | link_freq = v4l2_get_link_freq(handler: csi2rx->source_subdev->ctrl_handler, |
185 | mul: fmt->bpp, div: 2 * csi2rx->num_lanes); |
186 | if (link_freq < 0) |
187 | return link_freq; |
188 | |
189 | ret = phy_mipi_dphy_get_default_config_for_hsclk(hs_clk_rate: link_freq, |
190 | lanes: csi2rx->num_lanes, cfg); |
191 | if (ret) |
192 | return ret; |
193 | |
194 | ret = phy_power_on(phy: csi2rx->dphy); |
195 | if (ret) |
196 | return ret; |
197 | |
198 | ret = phy_configure(phy: csi2rx->dphy, opts: &opts); |
199 | if (ret) { |
200 | phy_power_off(phy: csi2rx->dphy); |
201 | return ret; |
202 | } |
203 | |
204 | return 0; |
205 | } |
206 | |
207 | static int csi2rx_start(struct csi2rx_priv *csi2rx) |
208 | { |
209 | unsigned int i; |
210 | unsigned long lanes_used = 0; |
211 | u32 reg; |
212 | int ret; |
213 | |
214 | ret = clk_prepare_enable(clk: csi2rx->p_clk); |
215 | if (ret) |
216 | return ret; |
217 | |
218 | reset_control_deassert(rstc: csi2rx->p_rst); |
219 | csi2rx_reset(csi2rx); |
220 | |
221 | reg = csi2rx->num_lanes << 8; |
222 | for (i = 0; i < csi2rx->num_lanes; i++) { |
223 | reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, csi2rx->lanes[i]); |
224 | set_bit(nr: csi2rx->lanes[i], addr: &lanes_used); |
225 | } |
226 | |
227 | /* |
228 | * Even the unused lanes need to be mapped. In order to avoid |
229 | * to map twice to the same physical lane, keep the lanes used |
230 | * in the previous loop, and only map unused physical lanes to |
231 | * the rest of our logical lanes. |
232 | */ |
233 | for (i = csi2rx->num_lanes; i < csi2rx->max_lanes; i++) { |
234 | unsigned int idx = find_first_zero_bit(addr: &lanes_used, |
235 | size: csi2rx->max_lanes); |
236 | set_bit(nr: idx, addr: &lanes_used); |
237 | reg |= CSI2RX_STATIC_CFG_DLANE_MAP(i, i + 1); |
238 | } |
239 | |
240 | writel(val: reg, addr: csi2rx->base + CSI2RX_STATIC_CFG_REG); |
241 | |
242 | ret = v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, true); |
243 | if (ret) |
244 | goto err_disable_pclk; |
245 | |
246 | /* Enable DPHY clk and data lanes. */ |
247 | if (csi2rx->dphy) { |
248 | reg = CSI2RX_DPHY_CL_EN | CSI2RX_DPHY_CL_RST; |
249 | for (i = 0; i < csi2rx->num_lanes; i++) { |
250 | reg |= CSI2RX_DPHY_DL_EN(csi2rx->lanes[i] - 1); |
251 | reg |= CSI2RX_DPHY_DL_RST(csi2rx->lanes[i] - 1); |
252 | } |
253 | |
254 | writel(val: reg, addr: csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG); |
255 | } |
256 | |
257 | /* |
258 | * Create a static mapping between the CSI virtual channels |
259 | * and the output stream. |
260 | * |
261 | * This should be enhanced, but v4l2 lacks the support for |
262 | * changing that mapping dynamically. |
263 | * |
264 | * We also cannot enable and disable independent streams here, |
265 | * hence the reference counting. |
266 | */ |
267 | for (i = 0; i < csi2rx->max_streams; i++) { |
268 | ret = clk_prepare_enable(clk: csi2rx->pixel_clk[i]); |
269 | if (ret) |
270 | goto err_disable_pixclk; |
271 | |
272 | reset_control_deassert(rstc: csi2rx->pixel_rst[i]); |
273 | |
274 | writel(CSI2RX_STREAM_CFG_FIFO_MODE_LARGE_BUF, |
275 | addr: csi2rx->base + CSI2RX_STREAM_CFG_REG(i)); |
276 | |
277 | /* |
278 | * Enable one virtual channel. When multiple virtual channels |
279 | * are supported this will have to be changed. |
280 | */ |
281 | writel(CSI2RX_STREAM_DATA_CFG_VC_SELECT(0), |
282 | addr: csi2rx->base + CSI2RX_STREAM_DATA_CFG_REG(i)); |
283 | |
284 | writel(CSI2RX_STREAM_CTRL_START, |
285 | addr: csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); |
286 | } |
287 | |
288 | ret = clk_prepare_enable(clk: csi2rx->sys_clk); |
289 | if (ret) |
290 | goto err_disable_pixclk; |
291 | |
292 | reset_control_deassert(rstc: csi2rx->sys_rst); |
293 | |
294 | if (csi2rx->dphy) { |
295 | ret = csi2rx_configure_ext_dphy(csi2rx); |
296 | if (ret) { |
297 | dev_err(csi2rx->dev, |
298 | "Failed to configure external DPHY: %d\n" , ret); |
299 | goto err_disable_sysclk; |
300 | } |
301 | } |
302 | |
303 | clk_disable_unprepare(clk: csi2rx->p_clk); |
304 | |
305 | return 0; |
306 | |
307 | err_disable_sysclk: |
308 | clk_disable_unprepare(clk: csi2rx->sys_clk); |
309 | err_disable_pixclk: |
310 | for (; i > 0; i--) { |
311 | reset_control_assert(rstc: csi2rx->pixel_rst[i - 1]); |
312 | clk_disable_unprepare(clk: csi2rx->pixel_clk[i - 1]); |
313 | } |
314 | |
315 | err_disable_pclk: |
316 | clk_disable_unprepare(clk: csi2rx->p_clk); |
317 | |
318 | return ret; |
319 | } |
320 | |
321 | static void csi2rx_stop(struct csi2rx_priv *csi2rx) |
322 | { |
323 | unsigned int i; |
324 | u32 val; |
325 | int ret; |
326 | |
327 | clk_prepare_enable(clk: csi2rx->p_clk); |
328 | reset_control_assert(rstc: csi2rx->sys_rst); |
329 | clk_disable_unprepare(clk: csi2rx->sys_clk); |
330 | |
331 | for (i = 0; i < csi2rx->max_streams; i++) { |
332 | writel(CSI2RX_STREAM_CTRL_STOP, |
333 | addr: csi2rx->base + CSI2RX_STREAM_CTRL_REG(i)); |
334 | |
335 | ret = readl_relaxed_poll_timeout(csi2rx->base + |
336 | CSI2RX_STREAM_STATUS_REG(i), |
337 | val, |
338 | !(val & CSI2RX_STREAM_STATUS_RDY), |
339 | 10, 10000); |
340 | if (ret) |
341 | dev_warn(csi2rx->dev, |
342 | "Failed to stop streaming on pad%u\n" , i); |
343 | |
344 | reset_control_assert(rstc: csi2rx->pixel_rst[i]); |
345 | clk_disable_unprepare(clk: csi2rx->pixel_clk[i]); |
346 | } |
347 | |
348 | reset_control_assert(rstc: csi2rx->p_rst); |
349 | clk_disable_unprepare(clk: csi2rx->p_clk); |
350 | |
351 | if (v4l2_subdev_call(csi2rx->source_subdev, video, s_stream, false)) |
352 | dev_warn(csi2rx->dev, "Couldn't disable our subdev\n" ); |
353 | |
354 | if (csi2rx->dphy) { |
355 | writel(val: 0, addr: csi2rx->base + CSI2RX_DPHY_LANE_CTRL_REG); |
356 | |
357 | if (phy_power_off(phy: csi2rx->dphy)) |
358 | dev_warn(csi2rx->dev, "Couldn't power off DPHY\n" ); |
359 | } |
360 | } |
361 | |
362 | static int csi2rx_s_stream(struct v4l2_subdev *subdev, int enable) |
363 | { |
364 | struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); |
365 | int ret = 0; |
366 | |
367 | mutex_lock(&csi2rx->lock); |
368 | |
369 | if (enable) { |
370 | /* |
371 | * If we're not the first users, there's no need to |
372 | * enable the whole controller. |
373 | */ |
374 | if (!csi2rx->count) { |
375 | ret = csi2rx_start(csi2rx); |
376 | if (ret) |
377 | goto out; |
378 | } |
379 | |
380 | csi2rx->count++; |
381 | } else { |
382 | csi2rx->count--; |
383 | |
384 | /* |
385 | * Let the last user turn off the lights. |
386 | */ |
387 | if (!csi2rx->count) |
388 | csi2rx_stop(csi2rx); |
389 | } |
390 | |
391 | out: |
392 | mutex_unlock(lock: &csi2rx->lock); |
393 | return ret; |
394 | } |
395 | |
396 | static int csi2rx_enum_mbus_code(struct v4l2_subdev *subdev, |
397 | struct v4l2_subdev_state *state, |
398 | struct v4l2_subdev_mbus_code_enum *code_enum) |
399 | { |
400 | if (code_enum->index >= ARRAY_SIZE(formats)) |
401 | return -EINVAL; |
402 | |
403 | code_enum->code = formats[code_enum->index].code; |
404 | |
405 | return 0; |
406 | } |
407 | |
408 | static int csi2rx_set_fmt(struct v4l2_subdev *subdev, |
409 | struct v4l2_subdev_state *state, |
410 | struct v4l2_subdev_format *format) |
411 | { |
412 | struct v4l2_mbus_framefmt *fmt; |
413 | unsigned int i; |
414 | |
415 | /* No transcoding, source and sink formats must match. */ |
416 | if (format->pad != CSI2RX_PAD_SINK) |
417 | return v4l2_subdev_get_fmt(sd: subdev, state, format); |
418 | |
419 | if (!csi2rx_get_fmt_by_code(code: format->format.code)) |
420 | format->format.code = formats[0].code; |
421 | |
422 | format->format.field = V4L2_FIELD_NONE; |
423 | |
424 | /* Set sink format */ |
425 | fmt = v4l2_subdev_state_get_format(state, format->pad); |
426 | *fmt = format->format; |
427 | |
428 | /* Propagate to source formats */ |
429 | for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) { |
430 | fmt = v4l2_subdev_state_get_format(state, i); |
431 | *fmt = format->format; |
432 | } |
433 | |
434 | return 0; |
435 | } |
436 | |
437 | static int csi2rx_init_state(struct v4l2_subdev *subdev, |
438 | struct v4l2_subdev_state *state) |
439 | { |
440 | struct v4l2_subdev_format format = { |
441 | .pad = CSI2RX_PAD_SINK, |
442 | .format = { |
443 | .width = 640, |
444 | .height = 480, |
445 | .code = MEDIA_BUS_FMT_UYVY8_1X16, |
446 | .field = V4L2_FIELD_NONE, |
447 | .colorspace = V4L2_COLORSPACE_SRGB, |
448 | .ycbcr_enc = V4L2_YCBCR_ENC_601, |
449 | .quantization = V4L2_QUANTIZATION_LIM_RANGE, |
450 | .xfer_func = V4L2_XFER_FUNC_SRGB, |
451 | }, |
452 | }; |
453 | |
454 | return csi2rx_set_fmt(subdev, state, format: &format); |
455 | } |
456 | |
457 | static const struct v4l2_subdev_pad_ops csi2rx_pad_ops = { |
458 | .enum_mbus_code = csi2rx_enum_mbus_code, |
459 | .get_fmt = v4l2_subdev_get_fmt, |
460 | .set_fmt = csi2rx_set_fmt, |
461 | }; |
462 | |
463 | static const struct v4l2_subdev_video_ops csi2rx_video_ops = { |
464 | .s_stream = csi2rx_s_stream, |
465 | }; |
466 | |
467 | static const struct v4l2_subdev_ops csi2rx_subdev_ops = { |
468 | .video = &csi2rx_video_ops, |
469 | .pad = &csi2rx_pad_ops, |
470 | }; |
471 | |
472 | static const struct v4l2_subdev_internal_ops csi2rx_internal_ops = { |
473 | .init_state = csi2rx_init_state, |
474 | }; |
475 | |
476 | static const struct media_entity_operations csi2rx_media_ops = { |
477 | .link_validate = v4l2_subdev_link_validate, |
478 | }; |
479 | |
480 | static int csi2rx_async_bound(struct v4l2_async_notifier *notifier, |
481 | struct v4l2_subdev *s_subdev, |
482 | struct v4l2_async_connection *asd) |
483 | { |
484 | struct v4l2_subdev *subdev = notifier->sd; |
485 | struct csi2rx_priv *csi2rx = v4l2_subdev_to_csi2rx(subdev); |
486 | |
487 | csi2rx->source_pad = media_entity_get_fwnode_pad(entity: &s_subdev->entity, |
488 | fwnode: asd->match.fwnode, |
489 | MEDIA_PAD_FL_SOURCE); |
490 | if (csi2rx->source_pad < 0) { |
491 | dev_err(csi2rx->dev, "Couldn't find output pad for subdev %s\n" , |
492 | s_subdev->name); |
493 | return csi2rx->source_pad; |
494 | } |
495 | |
496 | csi2rx->source_subdev = s_subdev; |
497 | |
498 | dev_dbg(csi2rx->dev, "Bound %s pad: %d\n" , s_subdev->name, |
499 | csi2rx->source_pad); |
500 | |
501 | return media_create_pad_link(source: &csi2rx->source_subdev->entity, |
502 | source_pad: csi2rx->source_pad, |
503 | sink: &csi2rx->subdev.entity, sink_pad: 0, |
504 | MEDIA_LNK_FL_ENABLED | |
505 | MEDIA_LNK_FL_IMMUTABLE); |
506 | } |
507 | |
508 | static const struct v4l2_async_notifier_operations csi2rx_notifier_ops = { |
509 | .bound = csi2rx_async_bound, |
510 | }; |
511 | |
512 | static int csi2rx_get_resources(struct csi2rx_priv *csi2rx, |
513 | struct platform_device *pdev) |
514 | { |
515 | unsigned char i; |
516 | u32 dev_cfg; |
517 | int ret; |
518 | |
519 | csi2rx->base = devm_platform_ioremap_resource(pdev, index: 0); |
520 | if (IS_ERR(ptr: csi2rx->base)) |
521 | return PTR_ERR(ptr: csi2rx->base); |
522 | |
523 | csi2rx->sys_clk = devm_clk_get(dev: &pdev->dev, id: "sys_clk" ); |
524 | if (IS_ERR(ptr: csi2rx->sys_clk)) { |
525 | dev_err(&pdev->dev, "Couldn't get sys clock\n" ); |
526 | return PTR_ERR(ptr: csi2rx->sys_clk); |
527 | } |
528 | |
529 | csi2rx->p_clk = devm_clk_get(dev: &pdev->dev, id: "p_clk" ); |
530 | if (IS_ERR(ptr: csi2rx->p_clk)) { |
531 | dev_err(&pdev->dev, "Couldn't get P clock\n" ); |
532 | return PTR_ERR(ptr: csi2rx->p_clk); |
533 | } |
534 | |
535 | csi2rx->sys_rst = devm_reset_control_get_optional_exclusive(dev: &pdev->dev, |
536 | id: "sys" ); |
537 | if (IS_ERR(ptr: csi2rx->sys_rst)) |
538 | return PTR_ERR(ptr: csi2rx->sys_rst); |
539 | |
540 | csi2rx->p_rst = devm_reset_control_get_optional_exclusive(dev: &pdev->dev, |
541 | id: "reg_bank" ); |
542 | if (IS_ERR(ptr: csi2rx->p_rst)) |
543 | return PTR_ERR(ptr: csi2rx->p_rst); |
544 | |
545 | csi2rx->dphy = devm_phy_optional_get(dev: &pdev->dev, string: "dphy" ); |
546 | if (IS_ERR(ptr: csi2rx->dphy)) { |
547 | dev_err(&pdev->dev, "Couldn't get external D-PHY\n" ); |
548 | return PTR_ERR(ptr: csi2rx->dphy); |
549 | } |
550 | |
551 | ret = clk_prepare_enable(clk: csi2rx->p_clk); |
552 | if (ret) { |
553 | dev_err(&pdev->dev, "Couldn't prepare and enable P clock\n" ); |
554 | return ret; |
555 | } |
556 | |
557 | dev_cfg = readl(addr: csi2rx->base + CSI2RX_DEVICE_CFG_REG); |
558 | clk_disable_unprepare(clk: csi2rx->p_clk); |
559 | |
560 | csi2rx->max_lanes = dev_cfg & 7; |
561 | if (csi2rx->max_lanes > CSI2RX_LANES_MAX) { |
562 | dev_err(&pdev->dev, "Invalid number of lanes: %u\n" , |
563 | csi2rx->max_lanes); |
564 | return -EINVAL; |
565 | } |
566 | |
567 | csi2rx->max_streams = (dev_cfg >> 4) & 7; |
568 | if (csi2rx->max_streams > CSI2RX_STREAMS_MAX) { |
569 | dev_err(&pdev->dev, "Invalid number of streams: %u\n" , |
570 | csi2rx->max_streams); |
571 | return -EINVAL; |
572 | } |
573 | |
574 | csi2rx->has_internal_dphy = dev_cfg & BIT(3) ? true : false; |
575 | |
576 | /* |
577 | * FIXME: Once we'll have internal D-PHY support, the check |
578 | * will need to be removed. |
579 | */ |
580 | if (!csi2rx->dphy && csi2rx->has_internal_dphy) { |
581 | dev_err(&pdev->dev, "Internal D-PHY not supported yet\n" ); |
582 | return -EINVAL; |
583 | } |
584 | |
585 | for (i = 0; i < csi2rx->max_streams; i++) { |
586 | char name[16]; |
587 | |
588 | snprintf(buf: name, size: sizeof(name), fmt: "pixel_if%u_clk" , i); |
589 | csi2rx->pixel_clk[i] = devm_clk_get(dev: &pdev->dev, id: name); |
590 | if (IS_ERR(ptr: csi2rx->pixel_clk[i])) { |
591 | dev_err(&pdev->dev, "Couldn't get clock %s\n" , name); |
592 | return PTR_ERR(ptr: csi2rx->pixel_clk[i]); |
593 | } |
594 | |
595 | snprintf(buf: name, size: sizeof(name), fmt: "pixel_if%u" , i); |
596 | csi2rx->pixel_rst[i] = |
597 | devm_reset_control_get_optional_exclusive(dev: &pdev->dev, |
598 | id: name); |
599 | if (IS_ERR(ptr: csi2rx->pixel_rst[i])) |
600 | return PTR_ERR(ptr: csi2rx->pixel_rst[i]); |
601 | } |
602 | |
603 | return 0; |
604 | } |
605 | |
606 | static int csi2rx_parse_dt(struct csi2rx_priv *csi2rx) |
607 | { |
608 | struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 }; |
609 | struct v4l2_async_connection *asd; |
610 | struct fwnode_handle *fwh; |
611 | struct device_node *ep; |
612 | int ret; |
613 | |
614 | ep = of_graph_get_endpoint_by_regs(parent: csi2rx->dev->of_node, port_reg: 0, reg: 0); |
615 | if (!ep) |
616 | return -EINVAL; |
617 | |
618 | fwh = of_fwnode_handle(ep); |
619 | ret = v4l2_fwnode_endpoint_parse(fwnode: fwh, vep: &v4l2_ep); |
620 | if (ret) { |
621 | dev_err(csi2rx->dev, "Could not parse v4l2 endpoint\n" ); |
622 | of_node_put(node: ep); |
623 | return ret; |
624 | } |
625 | |
626 | if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) { |
627 | dev_err(csi2rx->dev, "Unsupported media bus type: 0x%x\n" , |
628 | v4l2_ep.bus_type); |
629 | of_node_put(node: ep); |
630 | return -EINVAL; |
631 | } |
632 | |
633 | memcpy(csi2rx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes, |
634 | sizeof(csi2rx->lanes)); |
635 | csi2rx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes; |
636 | if (csi2rx->num_lanes > csi2rx->max_lanes) { |
637 | dev_err(csi2rx->dev, "Unsupported number of data-lanes: %d\n" , |
638 | csi2rx->num_lanes); |
639 | of_node_put(node: ep); |
640 | return -EINVAL; |
641 | } |
642 | |
643 | v4l2_async_subdev_nf_init(notifier: &csi2rx->notifier, sd: &csi2rx->subdev); |
644 | |
645 | asd = v4l2_async_nf_add_fwnode_remote(&csi2rx->notifier, fwh, |
646 | struct v4l2_async_connection); |
647 | of_node_put(node: ep); |
648 | if (IS_ERR(ptr: asd)) { |
649 | v4l2_async_nf_cleanup(notifier: &csi2rx->notifier); |
650 | return PTR_ERR(ptr: asd); |
651 | } |
652 | |
653 | csi2rx->notifier.ops = &csi2rx_notifier_ops; |
654 | |
655 | ret = v4l2_async_nf_register(notifier: &csi2rx->notifier); |
656 | if (ret) |
657 | v4l2_async_nf_cleanup(notifier: &csi2rx->notifier); |
658 | |
659 | return ret; |
660 | } |
661 | |
662 | static int csi2rx_probe(struct platform_device *pdev) |
663 | { |
664 | struct csi2rx_priv *csi2rx; |
665 | unsigned int i; |
666 | int ret; |
667 | |
668 | csi2rx = kzalloc(size: sizeof(*csi2rx), GFP_KERNEL); |
669 | if (!csi2rx) |
670 | return -ENOMEM; |
671 | platform_set_drvdata(pdev, data: csi2rx); |
672 | csi2rx->dev = &pdev->dev; |
673 | mutex_init(&csi2rx->lock); |
674 | |
675 | ret = csi2rx_get_resources(csi2rx, pdev); |
676 | if (ret) |
677 | goto err_free_priv; |
678 | |
679 | ret = csi2rx_parse_dt(csi2rx); |
680 | if (ret) |
681 | goto err_free_priv; |
682 | |
683 | csi2rx->subdev.owner = THIS_MODULE; |
684 | csi2rx->subdev.dev = &pdev->dev; |
685 | v4l2_subdev_init(sd: &csi2rx->subdev, ops: &csi2rx_subdev_ops); |
686 | csi2rx->subdev.internal_ops = &csi2rx_internal_ops; |
687 | v4l2_set_subdevdata(sd: &csi2rx->subdev, p: &pdev->dev); |
688 | snprintf(buf: csi2rx->subdev.name, size: sizeof(csi2rx->subdev.name), |
689 | fmt: "%s.%s" , KBUILD_MODNAME, dev_name(dev: &pdev->dev)); |
690 | |
691 | /* Create our media pads */ |
692 | csi2rx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; |
693 | csi2rx->pads[CSI2RX_PAD_SINK].flags = MEDIA_PAD_FL_SINK; |
694 | for (i = CSI2RX_PAD_SOURCE_STREAM0; i < CSI2RX_PAD_MAX; i++) |
695 | csi2rx->pads[i].flags = MEDIA_PAD_FL_SOURCE; |
696 | csi2rx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
697 | csi2rx->subdev.entity.ops = &csi2rx_media_ops; |
698 | |
699 | ret = media_entity_pads_init(entity: &csi2rx->subdev.entity, num_pads: CSI2RX_PAD_MAX, |
700 | pads: csi2rx->pads); |
701 | if (ret) |
702 | goto err_cleanup; |
703 | |
704 | ret = v4l2_subdev_init_finalize(&csi2rx->subdev); |
705 | if (ret) |
706 | goto err_cleanup; |
707 | |
708 | ret = v4l2_async_register_subdev(sd: &csi2rx->subdev); |
709 | if (ret < 0) |
710 | goto err_free_state; |
711 | |
712 | dev_info(&pdev->dev, |
713 | "Probed CSI2RX with %u/%u lanes, %u streams, %s D-PHY\n" , |
714 | csi2rx->num_lanes, csi2rx->max_lanes, csi2rx->max_streams, |
715 | csi2rx->dphy ? "external" : |
716 | csi2rx->has_internal_dphy ? "internal" : "no" ); |
717 | |
718 | return 0; |
719 | |
720 | err_free_state: |
721 | v4l2_subdev_cleanup(sd: &csi2rx->subdev); |
722 | err_cleanup: |
723 | v4l2_async_nf_unregister(notifier: &csi2rx->notifier); |
724 | v4l2_async_nf_cleanup(notifier: &csi2rx->notifier); |
725 | media_entity_cleanup(entity: &csi2rx->subdev.entity); |
726 | err_free_priv: |
727 | kfree(objp: csi2rx); |
728 | return ret; |
729 | } |
730 | |
731 | static void csi2rx_remove(struct platform_device *pdev) |
732 | { |
733 | struct csi2rx_priv *csi2rx = platform_get_drvdata(pdev); |
734 | |
735 | v4l2_async_nf_unregister(notifier: &csi2rx->notifier); |
736 | v4l2_async_nf_cleanup(notifier: &csi2rx->notifier); |
737 | v4l2_async_unregister_subdev(sd: &csi2rx->subdev); |
738 | v4l2_subdev_cleanup(sd: &csi2rx->subdev); |
739 | media_entity_cleanup(entity: &csi2rx->subdev.entity); |
740 | kfree(objp: csi2rx); |
741 | } |
742 | |
743 | static const struct of_device_id csi2rx_of_table[] = { |
744 | { .compatible = "starfive,jh7110-csi2rx" }, |
745 | { .compatible = "cdns,csi2rx" }, |
746 | { }, |
747 | }; |
748 | MODULE_DEVICE_TABLE(of, csi2rx_of_table); |
749 | |
750 | static struct platform_driver csi2rx_driver = { |
751 | .probe = csi2rx_probe, |
752 | .remove_new = csi2rx_remove, |
753 | |
754 | .driver = { |
755 | .name = "cdns-csi2rx" , |
756 | .of_match_table = csi2rx_of_table, |
757 | }, |
758 | }; |
759 | module_platform_driver(csi2rx_driver); |
760 | MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>" ); |
761 | MODULE_DESCRIPTION("Cadence CSI2-RX controller" ); |
762 | MODULE_LICENSE("GPL" ); |
763 | |