1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * Driver for Cadence MIPI-CSI2 TX Controller |
4 | * |
5 | * Copyright (C) 2017-2019 Cadence Design Systems Inc. |
6 | */ |
7 | |
8 | #include <linux/clk.h> |
9 | #include <linux/delay.h> |
10 | #include <linux/io.h> |
11 | #include <linux/module.h> |
12 | #include <linux/mutex.h> |
13 | #include <linux/of.h> |
14 | #include <linux/of_graph.h> |
15 | #include <linux/platform_device.h> |
16 | #include <linux/slab.h> |
17 | |
18 | #include <media/mipi-csi2.h> |
19 | #include <media/v4l2-ctrls.h> |
20 | #include <media/v4l2-device.h> |
21 | #include <media/v4l2-fwnode.h> |
22 | #include <media/v4l2-subdev.h> |
23 | |
24 | #define CSI2TX_DEVICE_CONFIG_REG 0x00 |
25 | #define CSI2TX_DEVICE_CONFIG_STREAMS_MASK GENMASK(6, 4) |
26 | #define CSI2TX_DEVICE_CONFIG_HAS_DPHY BIT(3) |
27 | #define CSI2TX_DEVICE_CONFIG_LANES_MASK GENMASK(2, 0) |
28 | |
29 | #define CSI2TX_CONFIG_REG 0x20 |
30 | #define CSI2TX_CONFIG_CFG_REQ BIT(2) |
31 | #define CSI2TX_CONFIG_SRST_REQ BIT(1) |
32 | |
33 | #define CSI2TX_DPHY_CFG_REG 0x28 |
34 | #define CSI2TX_DPHY_CFG_CLK_RESET BIT(16) |
35 | #define CSI2TX_DPHY_CFG_LANE_RESET(n) BIT((n) + 12) |
36 | #define CSI2TX_DPHY_CFG_MODE_MASK GENMASK(9, 8) |
37 | #define CSI2TX_DPHY_CFG_MODE_LPDT (2 << 8) |
38 | #define CSI2TX_DPHY_CFG_MODE_HS (1 << 8) |
39 | #define CSI2TX_DPHY_CFG_MODE_ULPS (0 << 8) |
40 | #define CSI2TX_DPHY_CFG_CLK_ENABLE BIT(4) |
41 | #define CSI2TX_DPHY_CFG_LANE_ENABLE(n) BIT(n) |
42 | |
43 | #define CSI2TX_DPHY_CLK_WAKEUP_REG 0x2c |
44 | #define CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(n) ((n) & 0xffff) |
45 | |
46 | #define CSI2TX_DT_CFG_REG(n) (0x80 + (n) * 8) |
47 | #define CSI2TX_DT_CFG_DT(n) (((n) & 0x3f) << 2) |
48 | |
49 | #define CSI2TX_DT_FORMAT_REG(n) (0x84 + (n) * 8) |
50 | #define CSI2TX_DT_FORMAT_BYTES_PER_LINE(n) (((n) & 0xffff) << 16) |
51 | #define CSI2TX_DT_FORMAT_MAX_LINE_NUM(n) ((n) & 0xffff) |
52 | |
53 | #define CSI2TX_STREAM_IF_CFG_REG(n) (0x100 + (n) * 4) |
54 | #define CSI2TX_STREAM_IF_CFG_FILL_LEVEL(n) ((n) & 0x1f) |
55 | |
56 | /* CSI2TX V2 Registers */ |
57 | #define CSI2TX_V2_DPHY_CFG_REG 0x28 |
58 | #define CSI2TX_V2_DPHY_CFG_RESET BIT(16) |
59 | #define CSI2TX_V2_DPHY_CFG_CLOCK_MODE BIT(10) |
60 | #define CSI2TX_V2_DPHY_CFG_MODE_MASK GENMASK(9, 8) |
61 | #define CSI2TX_V2_DPHY_CFG_MODE_LPDT (2 << 8) |
62 | #define CSI2TX_V2_DPHY_CFG_MODE_HS (1 << 8) |
63 | #define CSI2TX_V2_DPHY_CFG_MODE_ULPS (0 << 8) |
64 | #define CSI2TX_V2_DPHY_CFG_CLK_ENABLE BIT(4) |
65 | #define CSI2TX_V2_DPHY_CFG_LANE_ENABLE(n) BIT(n) |
66 | |
67 | #define CSI2TX_LANES_MAX 4 |
68 | #define CSI2TX_STREAMS_MAX 4 |
69 | |
70 | enum csi2tx_pads { |
71 | CSI2TX_PAD_SOURCE, |
72 | CSI2TX_PAD_SINK_STREAM0, |
73 | CSI2TX_PAD_SINK_STREAM1, |
74 | CSI2TX_PAD_SINK_STREAM2, |
75 | CSI2TX_PAD_SINK_STREAM3, |
76 | CSI2TX_PAD_MAX, |
77 | }; |
78 | |
79 | struct csi2tx_fmt { |
80 | u32 mbus; |
81 | u32 dt; |
82 | u32 bpp; |
83 | }; |
84 | |
85 | struct csi2tx_priv; |
86 | |
87 | /* CSI2TX Variant Operations */ |
88 | struct csi2tx_vops { |
89 | void (*dphy_setup)(struct csi2tx_priv *csi2tx); |
90 | }; |
91 | |
92 | struct csi2tx_priv { |
93 | struct device *dev; |
94 | unsigned int count; |
95 | |
96 | /* |
97 | * Used to prevent race conditions between multiple, |
98 | * concurrent calls to start and stop. |
99 | */ |
100 | struct mutex lock; |
101 | |
102 | void __iomem *base; |
103 | |
104 | struct csi2tx_vops *vops; |
105 | |
106 | struct clk *esc_clk; |
107 | struct clk *p_clk; |
108 | struct clk *pixel_clk[CSI2TX_STREAMS_MAX]; |
109 | |
110 | struct v4l2_subdev subdev; |
111 | struct media_pad pads[CSI2TX_PAD_MAX]; |
112 | struct v4l2_mbus_framefmt pad_fmts[CSI2TX_PAD_MAX]; |
113 | |
114 | bool has_internal_dphy; |
115 | u8 lanes[CSI2TX_LANES_MAX]; |
116 | unsigned int num_lanes; |
117 | unsigned int max_lanes; |
118 | unsigned int max_streams; |
119 | }; |
120 | |
121 | static const struct csi2tx_fmt csi2tx_formats[] = { |
122 | { |
123 | .mbus = MEDIA_BUS_FMT_UYVY8_1X16, |
124 | .bpp = 2, |
125 | .dt = MIPI_CSI2_DT_YUV422_8B, |
126 | }, |
127 | { |
128 | .mbus = MEDIA_BUS_FMT_RGB888_1X24, |
129 | .bpp = 3, |
130 | .dt = MIPI_CSI2_DT_RGB888, |
131 | }, |
132 | }; |
133 | |
134 | static const struct v4l2_mbus_framefmt fmt_default = { |
135 | .width = 1280, |
136 | .height = 720, |
137 | .code = MEDIA_BUS_FMT_RGB888_1X24, |
138 | .field = V4L2_FIELD_NONE, |
139 | .colorspace = V4L2_COLORSPACE_DEFAULT, |
140 | }; |
141 | |
142 | static inline |
143 | struct csi2tx_priv *v4l2_subdev_to_csi2tx(struct v4l2_subdev *subdev) |
144 | { |
145 | return container_of(subdev, struct csi2tx_priv, subdev); |
146 | } |
147 | |
148 | static const struct csi2tx_fmt *csi2tx_get_fmt_from_mbus(u32 mbus) |
149 | { |
150 | unsigned int i; |
151 | |
152 | for (i = 0; i < ARRAY_SIZE(csi2tx_formats); i++) |
153 | if (csi2tx_formats[i].mbus == mbus) |
154 | return &csi2tx_formats[i]; |
155 | |
156 | return NULL; |
157 | } |
158 | |
159 | static int csi2tx_enum_mbus_code(struct v4l2_subdev *subdev, |
160 | struct v4l2_subdev_state *sd_state, |
161 | struct v4l2_subdev_mbus_code_enum *code) |
162 | { |
163 | if (code->pad || code->index >= ARRAY_SIZE(csi2tx_formats)) |
164 | return -EINVAL; |
165 | |
166 | code->code = csi2tx_formats[code->index].mbus; |
167 | |
168 | return 0; |
169 | } |
170 | |
171 | static struct v4l2_mbus_framefmt * |
172 | __csi2tx_get_pad_format(struct v4l2_subdev *subdev, |
173 | struct v4l2_subdev_state *sd_state, |
174 | struct v4l2_subdev_format *fmt) |
175 | { |
176 | struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev); |
177 | |
178 | if (fmt->which == V4L2_SUBDEV_FORMAT_TRY) |
179 | return v4l2_subdev_state_get_format(sd_state, fmt->pad); |
180 | |
181 | return &csi2tx->pad_fmts[fmt->pad]; |
182 | } |
183 | |
184 | static int csi2tx_get_pad_format(struct v4l2_subdev *subdev, |
185 | struct v4l2_subdev_state *sd_state, |
186 | struct v4l2_subdev_format *fmt) |
187 | { |
188 | const struct v4l2_mbus_framefmt *format; |
189 | |
190 | /* Multiplexed pad? */ |
191 | if (fmt->pad == CSI2TX_PAD_SOURCE) |
192 | return -EINVAL; |
193 | |
194 | format = __csi2tx_get_pad_format(subdev, sd_state, fmt); |
195 | if (!format) |
196 | return -EINVAL; |
197 | |
198 | fmt->format = *format; |
199 | |
200 | return 0; |
201 | } |
202 | |
203 | static int csi2tx_set_pad_format(struct v4l2_subdev *subdev, |
204 | struct v4l2_subdev_state *sd_state, |
205 | struct v4l2_subdev_format *fmt) |
206 | { |
207 | const struct v4l2_mbus_framefmt *src_format = &fmt->format; |
208 | struct v4l2_mbus_framefmt *dst_format; |
209 | |
210 | /* Multiplexed pad? */ |
211 | if (fmt->pad == CSI2TX_PAD_SOURCE) |
212 | return -EINVAL; |
213 | |
214 | if (!csi2tx_get_fmt_from_mbus(mbus: fmt->format.code)) |
215 | src_format = &fmt_default; |
216 | |
217 | dst_format = __csi2tx_get_pad_format(subdev, sd_state, fmt); |
218 | if (!dst_format) |
219 | return -EINVAL; |
220 | |
221 | *dst_format = *src_format; |
222 | |
223 | return 0; |
224 | } |
225 | |
226 | static const struct v4l2_subdev_pad_ops csi2tx_pad_ops = { |
227 | .enum_mbus_code = csi2tx_enum_mbus_code, |
228 | .get_fmt = csi2tx_get_pad_format, |
229 | .set_fmt = csi2tx_set_pad_format, |
230 | }; |
231 | |
232 | /* Set Wake Up value in the D-PHY */ |
233 | static void csi2tx_dphy_set_wakeup(struct csi2tx_priv *csi2tx) |
234 | { |
235 | writel(CSI2TX_DPHY_CLK_WAKEUP_ULPS_CYCLES(32), |
236 | addr: csi2tx->base + CSI2TX_DPHY_CLK_WAKEUP_REG); |
237 | } |
238 | |
239 | /* |
240 | * Finishes the D-PHY initialization |
241 | * reg dphy cfg value to be used |
242 | */ |
243 | static void csi2tx_dphy_init_finish(struct csi2tx_priv *csi2tx, u32 reg) |
244 | { |
245 | unsigned int i; |
246 | |
247 | udelay(10); |
248 | |
249 | /* Enable our (clock and data) lanes */ |
250 | reg |= CSI2TX_DPHY_CFG_CLK_ENABLE; |
251 | for (i = 0; i < csi2tx->num_lanes; i++) |
252 | reg |= CSI2TX_DPHY_CFG_LANE_ENABLE(csi2tx->lanes[i] - 1); |
253 | writel(val: reg, addr: csi2tx->base + CSI2TX_DPHY_CFG_REG); |
254 | |
255 | udelay(10); |
256 | |
257 | /* Switch to HS mode */ |
258 | reg &= ~CSI2TX_DPHY_CFG_MODE_MASK; |
259 | writel(val: reg | CSI2TX_DPHY_CFG_MODE_HS, |
260 | addr: csi2tx->base + CSI2TX_DPHY_CFG_REG); |
261 | } |
262 | |
263 | /* Configures D-PHY in CSIv1.3 */ |
264 | static void csi2tx_dphy_setup(struct csi2tx_priv *csi2tx) |
265 | { |
266 | u32 reg; |
267 | unsigned int i; |
268 | |
269 | csi2tx_dphy_set_wakeup(csi2tx); |
270 | |
271 | /* Put our lanes (clock and data) out of reset */ |
272 | reg = CSI2TX_DPHY_CFG_CLK_RESET | CSI2TX_DPHY_CFG_MODE_LPDT; |
273 | for (i = 0; i < csi2tx->num_lanes; i++) |
274 | reg |= CSI2TX_DPHY_CFG_LANE_RESET(csi2tx->lanes[i] - 1); |
275 | writel(val: reg, addr: csi2tx->base + CSI2TX_DPHY_CFG_REG); |
276 | |
277 | csi2tx_dphy_init_finish(csi2tx, reg); |
278 | } |
279 | |
280 | /* Configures D-PHY in CSIv2 */ |
281 | static void csi2tx_v2_dphy_setup(struct csi2tx_priv *csi2tx) |
282 | { |
283 | u32 reg; |
284 | |
285 | csi2tx_dphy_set_wakeup(csi2tx); |
286 | |
287 | /* Put our lanes (clock and data) out of reset */ |
288 | reg = CSI2TX_V2_DPHY_CFG_RESET | CSI2TX_V2_DPHY_CFG_MODE_LPDT; |
289 | writel(val: reg, addr: csi2tx->base + CSI2TX_V2_DPHY_CFG_REG); |
290 | |
291 | csi2tx_dphy_init_finish(csi2tx, reg); |
292 | } |
293 | |
294 | static void csi2tx_reset(struct csi2tx_priv *csi2tx) |
295 | { |
296 | writel(CSI2TX_CONFIG_SRST_REQ, addr: csi2tx->base + CSI2TX_CONFIG_REG); |
297 | |
298 | udelay(10); |
299 | } |
300 | |
301 | static int csi2tx_start(struct csi2tx_priv *csi2tx) |
302 | { |
303 | struct media_entity *entity = &csi2tx->subdev.entity; |
304 | struct media_link *link; |
305 | unsigned int i; |
306 | |
307 | csi2tx_reset(csi2tx); |
308 | |
309 | writel(CSI2TX_CONFIG_CFG_REQ, addr: csi2tx->base + CSI2TX_CONFIG_REG); |
310 | |
311 | udelay(10); |
312 | |
313 | if (csi2tx->vops && csi2tx->vops->dphy_setup) { |
314 | csi2tx->vops->dphy_setup(csi2tx); |
315 | udelay(10); |
316 | } |
317 | |
318 | /* |
319 | * Create a static mapping between the CSI virtual channels |
320 | * and the input streams. |
321 | * |
322 | * This should be enhanced, but v4l2 lacks the support for |
323 | * changing that mapping dynamically at the moment. |
324 | * |
325 | * We're protected from the userspace setting up links at the |
326 | * same time by the upper layer having called |
327 | * media_pipeline_start(). |
328 | */ |
329 | list_for_each_entry(link, &entity->links, list) { |
330 | struct v4l2_mbus_framefmt *mfmt; |
331 | const struct csi2tx_fmt *fmt; |
332 | unsigned int stream; |
333 | int pad_idx = -1; |
334 | |
335 | /* Only consider our enabled input pads */ |
336 | for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) { |
337 | struct media_pad *pad = &csi2tx->pads[i]; |
338 | |
339 | if ((pad == link->sink) && |
340 | (link->flags & MEDIA_LNK_FL_ENABLED)) { |
341 | pad_idx = i; |
342 | break; |
343 | } |
344 | } |
345 | |
346 | if (pad_idx < 0) |
347 | continue; |
348 | |
349 | mfmt = &csi2tx->pad_fmts[pad_idx]; |
350 | fmt = csi2tx_get_fmt_from_mbus(mbus: mfmt->code); |
351 | if (!fmt) |
352 | continue; |
353 | |
354 | stream = pad_idx - CSI2TX_PAD_SINK_STREAM0; |
355 | |
356 | /* |
357 | * We use the stream ID there, but it's wrong. |
358 | * |
359 | * A stream could very well send a data type that is |
360 | * not equal to its stream ID. We need to find a |
361 | * proper way to address it. |
362 | */ |
363 | writel(CSI2TX_DT_CFG_DT(fmt->dt), |
364 | addr: csi2tx->base + CSI2TX_DT_CFG_REG(stream)); |
365 | |
366 | writel(CSI2TX_DT_FORMAT_BYTES_PER_LINE(mfmt->width * fmt->bpp) | |
367 | CSI2TX_DT_FORMAT_MAX_LINE_NUM(mfmt->height + 1), |
368 | addr: csi2tx->base + CSI2TX_DT_FORMAT_REG(stream)); |
369 | |
370 | /* |
371 | * TODO: This needs to be calculated based on the |
372 | * output CSI2 clock rate. |
373 | */ |
374 | writel(CSI2TX_STREAM_IF_CFG_FILL_LEVEL(4), |
375 | addr: csi2tx->base + CSI2TX_STREAM_IF_CFG_REG(stream)); |
376 | } |
377 | |
378 | /* Disable the configuration mode */ |
379 | writel(val: 0, addr: csi2tx->base + CSI2TX_CONFIG_REG); |
380 | |
381 | return 0; |
382 | } |
383 | |
384 | static void csi2tx_stop(struct csi2tx_priv *csi2tx) |
385 | { |
386 | writel(CSI2TX_CONFIG_CFG_REQ | CSI2TX_CONFIG_SRST_REQ, |
387 | addr: csi2tx->base + CSI2TX_CONFIG_REG); |
388 | } |
389 | |
390 | static int csi2tx_s_stream(struct v4l2_subdev *subdev, int enable) |
391 | { |
392 | struct csi2tx_priv *csi2tx = v4l2_subdev_to_csi2tx(subdev); |
393 | int ret = 0; |
394 | |
395 | mutex_lock(&csi2tx->lock); |
396 | |
397 | if (enable) { |
398 | /* |
399 | * If we're not the first users, there's no need to |
400 | * enable the whole controller. |
401 | */ |
402 | if (!csi2tx->count) { |
403 | ret = csi2tx_start(csi2tx); |
404 | if (ret) |
405 | goto out; |
406 | } |
407 | |
408 | csi2tx->count++; |
409 | } else { |
410 | csi2tx->count--; |
411 | |
412 | /* |
413 | * Let the last user turn off the lights. |
414 | */ |
415 | if (!csi2tx->count) |
416 | csi2tx_stop(csi2tx); |
417 | } |
418 | |
419 | out: |
420 | mutex_unlock(lock: &csi2tx->lock); |
421 | return ret; |
422 | } |
423 | |
424 | static const struct v4l2_subdev_video_ops csi2tx_video_ops = { |
425 | .s_stream = csi2tx_s_stream, |
426 | }; |
427 | |
428 | static const struct v4l2_subdev_ops csi2tx_subdev_ops = { |
429 | .pad = &csi2tx_pad_ops, |
430 | .video = &csi2tx_video_ops, |
431 | }; |
432 | |
433 | static int csi2tx_get_resources(struct csi2tx_priv *csi2tx, |
434 | struct platform_device *pdev) |
435 | { |
436 | unsigned int i; |
437 | u32 dev_cfg; |
438 | int ret; |
439 | |
440 | csi2tx->base = devm_platform_ioremap_resource(pdev, index: 0); |
441 | if (IS_ERR(ptr: csi2tx->base)) |
442 | return PTR_ERR(ptr: csi2tx->base); |
443 | |
444 | csi2tx->p_clk = devm_clk_get(dev: &pdev->dev, id: "p_clk" ); |
445 | if (IS_ERR(ptr: csi2tx->p_clk)) { |
446 | dev_err(&pdev->dev, "Couldn't get p_clk\n" ); |
447 | return PTR_ERR(ptr: csi2tx->p_clk); |
448 | } |
449 | |
450 | csi2tx->esc_clk = devm_clk_get(dev: &pdev->dev, id: "esc_clk" ); |
451 | if (IS_ERR(ptr: csi2tx->esc_clk)) { |
452 | dev_err(&pdev->dev, "Couldn't get the esc_clk\n" ); |
453 | return PTR_ERR(ptr: csi2tx->esc_clk); |
454 | } |
455 | |
456 | ret = clk_prepare_enable(clk: csi2tx->p_clk); |
457 | if (ret) { |
458 | dev_err(&pdev->dev, "Couldn't prepare and enable p_clk\n" ); |
459 | return ret; |
460 | } |
461 | |
462 | dev_cfg = readl(addr: csi2tx->base + CSI2TX_DEVICE_CONFIG_REG); |
463 | clk_disable_unprepare(clk: csi2tx->p_clk); |
464 | |
465 | csi2tx->max_lanes = dev_cfg & CSI2TX_DEVICE_CONFIG_LANES_MASK; |
466 | if (csi2tx->max_lanes > CSI2TX_LANES_MAX) { |
467 | dev_err(&pdev->dev, "Invalid number of lanes: %u\n" , |
468 | csi2tx->max_lanes); |
469 | return -EINVAL; |
470 | } |
471 | |
472 | csi2tx->max_streams = (dev_cfg & CSI2TX_DEVICE_CONFIG_STREAMS_MASK) >> 4; |
473 | if (csi2tx->max_streams > CSI2TX_STREAMS_MAX) { |
474 | dev_err(&pdev->dev, "Invalid number of streams: %u\n" , |
475 | csi2tx->max_streams); |
476 | return -EINVAL; |
477 | } |
478 | |
479 | csi2tx->has_internal_dphy = !!(dev_cfg & CSI2TX_DEVICE_CONFIG_HAS_DPHY); |
480 | |
481 | for (i = 0; i < csi2tx->max_streams; i++) { |
482 | char clk_name[23]; |
483 | |
484 | snprintf(buf: clk_name, size: sizeof(clk_name), fmt: "pixel_if%u_clk" , i); |
485 | csi2tx->pixel_clk[i] = devm_clk_get(dev: &pdev->dev, id: clk_name); |
486 | if (IS_ERR(ptr: csi2tx->pixel_clk[i])) { |
487 | dev_err(&pdev->dev, "Couldn't get clock %s\n" , |
488 | clk_name); |
489 | return PTR_ERR(ptr: csi2tx->pixel_clk[i]); |
490 | } |
491 | } |
492 | |
493 | return 0; |
494 | } |
495 | |
496 | static int csi2tx_check_lanes(struct csi2tx_priv *csi2tx) |
497 | { |
498 | struct v4l2_fwnode_endpoint v4l2_ep = { .bus_type = 0 }; |
499 | struct device_node *ep; |
500 | int ret, i; |
501 | |
502 | ep = of_graph_get_endpoint_by_regs(parent: csi2tx->dev->of_node, port_reg: 0, reg: 0); |
503 | if (!ep) |
504 | return -EINVAL; |
505 | |
506 | ret = v4l2_fwnode_endpoint_parse(of_fwnode_handle(ep), vep: &v4l2_ep); |
507 | if (ret) { |
508 | dev_err(csi2tx->dev, "Could not parse v4l2 endpoint\n" ); |
509 | goto out; |
510 | } |
511 | |
512 | if (v4l2_ep.bus_type != V4L2_MBUS_CSI2_DPHY) { |
513 | dev_err(csi2tx->dev, "Unsupported media bus type: 0x%x\n" , |
514 | v4l2_ep.bus_type); |
515 | ret = -EINVAL; |
516 | goto out; |
517 | } |
518 | |
519 | csi2tx->num_lanes = v4l2_ep.bus.mipi_csi2.num_data_lanes; |
520 | if (csi2tx->num_lanes > csi2tx->max_lanes) { |
521 | dev_err(csi2tx->dev, |
522 | "Current configuration uses more lanes than supported\n" ); |
523 | ret = -EINVAL; |
524 | goto out; |
525 | } |
526 | |
527 | for (i = 0; i < csi2tx->num_lanes; i++) { |
528 | if (v4l2_ep.bus.mipi_csi2.data_lanes[i] < 1) { |
529 | dev_err(csi2tx->dev, "Invalid lane[%d] number: %u\n" , |
530 | i, v4l2_ep.bus.mipi_csi2.data_lanes[i]); |
531 | ret = -EINVAL; |
532 | goto out; |
533 | } |
534 | } |
535 | |
536 | memcpy(csi2tx->lanes, v4l2_ep.bus.mipi_csi2.data_lanes, |
537 | sizeof(csi2tx->lanes)); |
538 | |
539 | out: |
540 | of_node_put(node: ep); |
541 | return ret; |
542 | } |
543 | |
544 | static const struct csi2tx_vops csi2tx_vops = { |
545 | .dphy_setup = csi2tx_dphy_setup, |
546 | }; |
547 | |
548 | static const struct csi2tx_vops csi2tx_v2_vops = { |
549 | .dphy_setup = csi2tx_v2_dphy_setup, |
550 | }; |
551 | |
552 | static const struct of_device_id csi2tx_of_table[] = { |
553 | { |
554 | .compatible = "cdns,csi2tx" , |
555 | .data = &csi2tx_vops |
556 | }, |
557 | { |
558 | .compatible = "cdns,csi2tx-1.3" , |
559 | .data = &csi2tx_vops |
560 | }, |
561 | { |
562 | .compatible = "cdns,csi2tx-2.1" , |
563 | .data = &csi2tx_v2_vops |
564 | }, |
565 | { } |
566 | }; |
567 | MODULE_DEVICE_TABLE(of, csi2tx_of_table); |
568 | |
569 | static int csi2tx_probe(struct platform_device *pdev) |
570 | { |
571 | struct csi2tx_priv *csi2tx; |
572 | const struct of_device_id *of_id; |
573 | unsigned int i; |
574 | int ret; |
575 | |
576 | csi2tx = kzalloc(size: sizeof(*csi2tx), GFP_KERNEL); |
577 | if (!csi2tx) |
578 | return -ENOMEM; |
579 | platform_set_drvdata(pdev, data: csi2tx); |
580 | mutex_init(&csi2tx->lock); |
581 | csi2tx->dev = &pdev->dev; |
582 | |
583 | ret = csi2tx_get_resources(csi2tx, pdev); |
584 | if (ret) |
585 | goto err_free_priv; |
586 | |
587 | of_id = of_match_node(matches: csi2tx_of_table, node: pdev->dev.of_node); |
588 | csi2tx->vops = (struct csi2tx_vops *)of_id->data; |
589 | |
590 | v4l2_subdev_init(sd: &csi2tx->subdev, ops: &csi2tx_subdev_ops); |
591 | csi2tx->subdev.owner = THIS_MODULE; |
592 | csi2tx->subdev.dev = &pdev->dev; |
593 | csi2tx->subdev.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE; |
594 | snprintf(buf: csi2tx->subdev.name, size: sizeof(csi2tx->subdev.name), |
595 | fmt: "%s.%s" , KBUILD_MODNAME, dev_name(dev: &pdev->dev)); |
596 | |
597 | ret = csi2tx_check_lanes(csi2tx); |
598 | if (ret) |
599 | goto err_free_priv; |
600 | |
601 | /* Create our media pads */ |
602 | csi2tx->subdev.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; |
603 | csi2tx->pads[CSI2TX_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE; |
604 | for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) |
605 | csi2tx->pads[i].flags = MEDIA_PAD_FL_SINK; |
606 | |
607 | /* |
608 | * Only the input pads are considered to have a format at the |
609 | * moment. The CSI link can multiplex various streams with |
610 | * different formats, and we can't expose this in v4l2 right |
611 | * now. |
612 | */ |
613 | for (i = CSI2TX_PAD_SINK_STREAM0; i < CSI2TX_PAD_MAX; i++) |
614 | csi2tx->pad_fmts[i] = fmt_default; |
615 | |
616 | ret = media_entity_pads_init(entity: &csi2tx->subdev.entity, num_pads: CSI2TX_PAD_MAX, |
617 | pads: csi2tx->pads); |
618 | if (ret) |
619 | goto err_free_priv; |
620 | |
621 | ret = v4l2_async_register_subdev(sd: &csi2tx->subdev); |
622 | if (ret < 0) |
623 | goto err_free_priv; |
624 | |
625 | dev_info(&pdev->dev, |
626 | "Probed CSI2TX with %u/%u lanes, %u streams, %s D-PHY\n" , |
627 | csi2tx->num_lanes, csi2tx->max_lanes, csi2tx->max_streams, |
628 | csi2tx->has_internal_dphy ? "internal" : "no" ); |
629 | |
630 | return 0; |
631 | |
632 | err_free_priv: |
633 | kfree(objp: csi2tx); |
634 | return ret; |
635 | } |
636 | |
637 | static void csi2tx_remove(struct platform_device *pdev) |
638 | { |
639 | struct csi2tx_priv *csi2tx = platform_get_drvdata(pdev); |
640 | |
641 | v4l2_async_unregister_subdev(sd: &csi2tx->subdev); |
642 | kfree(objp: csi2tx); |
643 | } |
644 | |
645 | static struct platform_driver csi2tx_driver = { |
646 | .probe = csi2tx_probe, |
647 | .remove_new = csi2tx_remove, |
648 | |
649 | .driver = { |
650 | .name = "cdns-csi2tx" , |
651 | .of_match_table = csi2tx_of_table, |
652 | }, |
653 | }; |
654 | module_platform_driver(csi2tx_driver); |
655 | MODULE_AUTHOR("Maxime Ripard <maxime.ripard@bootlin.com>" ); |
656 | MODULE_DESCRIPTION("Cadence CSI2-TX controller" ); |
657 | MODULE_LICENSE("GPL" ); |
658 | |