1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * MIPI CSI-2 Receiver Subdev for Freescale i.MX6 SOC. |
4 | * |
5 | * Copyright (c) 2012-2017 Mentor Graphics Inc. |
6 | */ |
7 | #include <linux/clk.h> |
8 | #include <linux/interrupt.h> |
9 | #include <linux/io.h> |
10 | #include <linux/iopoll.h> |
11 | #include <linux/irq.h> |
12 | #include <linux/module.h> |
13 | #include <linux/of_graph.h> |
14 | #include <linux/platform_device.h> |
15 | #include <media/v4l2-common.h> |
16 | #include <media/v4l2-device.h> |
17 | #include <media/v4l2-fwnode.h> |
18 | #include <media/v4l2-mc.h> |
19 | #include <media/v4l2-subdev.h> |
20 | #include "imx-media.h" |
21 | |
22 | /* |
23 | * there must be 5 pads: 1 input pad from sensor, and |
24 | * the 4 virtual channel output pads |
25 | */ |
26 | #define CSI2_SINK_PAD 0 |
27 | #define CSI2_NUM_SINK_PADS 1 |
28 | #define CSI2_NUM_SRC_PADS 4 |
29 | #define CSI2_NUM_PADS 5 |
30 | |
31 | /* |
32 | * The default maximum bit-rate per lane in Mbps, if the |
33 | * source subdev does not provide V4L2_CID_LINK_FREQ. |
34 | */ |
35 | #define CSI2_DEFAULT_MAX_MBPS 849 |
36 | |
37 | struct csi2_dev { |
38 | struct device *dev; |
39 | struct v4l2_subdev sd; |
40 | struct v4l2_async_notifier notifier; |
41 | struct media_pad pad[CSI2_NUM_PADS]; |
42 | struct clk *dphy_clk; |
43 | struct clk *pllref_clk; |
44 | struct clk *pix_clk; /* what is this? */ |
45 | void __iomem *base; |
46 | |
47 | struct v4l2_subdev *remote; |
48 | unsigned int remote_pad; |
49 | unsigned short data_lanes; |
50 | |
51 | /* lock to protect all members below */ |
52 | struct mutex lock; |
53 | |
54 | struct v4l2_mbus_framefmt format_mbus; |
55 | |
56 | int stream_count; |
57 | struct v4l2_subdev *src_sd; |
58 | bool sink_linked[CSI2_NUM_SRC_PADS]; |
59 | }; |
60 | |
61 | #define DEVICE_NAME "imx6-mipi-csi2" |
62 | |
63 | /* Register offsets */ |
64 | #define CSI2_VERSION 0x000 |
65 | #define CSI2_N_LANES 0x004 |
66 | #define CSI2_PHY_SHUTDOWNZ 0x008 |
67 | #define CSI2_DPHY_RSTZ 0x00c |
68 | #define CSI2_RESETN 0x010 |
69 | #define CSI2_PHY_STATE 0x014 |
70 | #define PHY_STOPSTATEDATA_BIT 4 |
71 | #define PHY_STOPSTATEDATA(n) BIT(PHY_STOPSTATEDATA_BIT + (n)) |
72 | #define PHY_RXCLKACTIVEHS BIT(8) |
73 | #define PHY_RXULPSCLKNOT BIT(9) |
74 | #define PHY_STOPSTATECLK BIT(10) |
75 | #define CSI2_DATA_IDS_1 0x018 |
76 | #define CSI2_DATA_IDS_2 0x01c |
77 | #define CSI2_ERR1 0x020 |
78 | #define CSI2_ERR2 0x024 |
79 | #define CSI2_MSK1 0x028 |
80 | #define CSI2_MSK2 0x02c |
81 | #define CSI2_PHY_TST_CTRL0 0x030 |
82 | #define PHY_TESTCLR BIT(0) |
83 | #define PHY_TESTCLK BIT(1) |
84 | #define CSI2_PHY_TST_CTRL1 0x034 |
85 | #define PHY_TESTEN BIT(16) |
86 | /* |
87 | * i.MX CSI2IPU Gasket registers follow. The CSI2IPU gasket is |
88 | * not part of the MIPI CSI-2 core, but its registers fall in the |
89 | * same register map range. |
90 | */ |
91 | #define CSI2IPU_GASKET 0xf00 |
92 | #define CSI2IPU_YUV422_YUYV BIT(2) |
93 | |
94 | static inline struct csi2_dev *sd_to_dev(struct v4l2_subdev *sdev) |
95 | { |
96 | return container_of(sdev, struct csi2_dev, sd); |
97 | } |
98 | |
99 | static inline struct csi2_dev *notifier_to_dev(struct v4l2_async_notifier *n) |
100 | { |
101 | return container_of(n, struct csi2_dev, notifier); |
102 | } |
103 | |
104 | /* |
105 | * The required sequence of MIPI CSI-2 startup as specified in the i.MX6 |
106 | * reference manual is as follows: |
107 | * |
108 | * 1. Deassert presetn signal (global reset). |
109 | * It's not clear what this "global reset" signal is (maybe APB |
110 | * global reset), but in any case this step would be probably |
111 | * be carried out during driver load in csi2_probe(). |
112 | * |
113 | * 2. Configure MIPI Camera Sensor to put all Tx lanes in LP-11 state. |
114 | * This must be carried out by the MIPI sensor's s_power(ON) subdev |
115 | * op. |
116 | * |
117 | * 3. D-PHY initialization. |
118 | * 4. CSI2 Controller programming (Set N_LANES, deassert PHY_SHUTDOWNZ, |
119 | * deassert PHY_RSTZ, deassert CSI2_RESETN). |
120 | * 5. Read the PHY status register (PHY_STATE) to confirm that all data and |
121 | * clock lanes of the D-PHY are in LP-11 state. |
122 | * 6. Configure the MIPI Camera Sensor to start transmitting a clock on the |
123 | * D-PHY clock lane. |
124 | * 7. CSI2 Controller programming - Read the PHY status register (PHY_STATE) |
125 | * to confirm that the D-PHY is receiving a clock on the D-PHY clock lane. |
126 | * |
127 | * All steps 3 through 7 are carried out by csi2_s_stream(ON) here. Step |
128 | * 6 is accomplished by calling the source subdev's s_stream(ON) between |
129 | * steps 5 and 7. |
130 | */ |
131 | |
132 | static void csi2_enable(struct csi2_dev *csi2, bool enable) |
133 | { |
134 | if (enable) { |
135 | writel(val: 0x1, addr: csi2->base + CSI2_PHY_SHUTDOWNZ); |
136 | writel(val: 0x1, addr: csi2->base + CSI2_DPHY_RSTZ); |
137 | writel(val: 0x1, addr: csi2->base + CSI2_RESETN); |
138 | } else { |
139 | writel(val: 0x0, addr: csi2->base + CSI2_PHY_SHUTDOWNZ); |
140 | writel(val: 0x0, addr: csi2->base + CSI2_DPHY_RSTZ); |
141 | writel(val: 0x0, addr: csi2->base + CSI2_RESETN); |
142 | } |
143 | } |
144 | |
145 | static void csi2_set_lanes(struct csi2_dev *csi2, unsigned int lanes) |
146 | { |
147 | writel(val: lanes - 1, addr: csi2->base + CSI2_N_LANES); |
148 | } |
149 | |
150 | static void dw_mipi_csi2_phy_write(struct csi2_dev *csi2, |
151 | u32 test_code, u32 test_data) |
152 | { |
153 | /* Clear PHY test interface */ |
154 | writel(PHY_TESTCLR, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
155 | writel(val: 0x0, addr: csi2->base + CSI2_PHY_TST_CTRL1); |
156 | writel(val: 0x0, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
157 | |
158 | /* Raise test interface strobe signal */ |
159 | writel(PHY_TESTCLK, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
160 | |
161 | /* Configure address write on falling edge and lower strobe signal */ |
162 | writel(PHY_TESTEN | test_code, addr: csi2->base + CSI2_PHY_TST_CTRL1); |
163 | writel(val: 0x0, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
164 | |
165 | /* Configure data write on rising edge and raise strobe signal */ |
166 | writel(val: test_data, addr: csi2->base + CSI2_PHY_TST_CTRL1); |
167 | writel(PHY_TESTCLK, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
168 | |
169 | /* Clear strobe signal */ |
170 | writel(val: 0x0, addr: csi2->base + CSI2_PHY_TST_CTRL0); |
171 | } |
172 | |
173 | /* |
174 | * This table is based on the table documented at |
175 | * https://community.nxp.com/docs/DOC-94312. It assumes |
176 | * a 27MHz D-PHY pll reference clock. |
177 | */ |
178 | static const struct { |
179 | u32 max_mbps; |
180 | u32 hsfreqrange_sel; |
181 | } hsfreq_map[] = { |
182 | { 90, 0x00}, {100, 0x20}, {110, 0x40}, {125, 0x02}, |
183 | {140, 0x22}, {150, 0x42}, {160, 0x04}, {180, 0x24}, |
184 | {200, 0x44}, {210, 0x06}, {240, 0x26}, {250, 0x46}, |
185 | {270, 0x08}, {300, 0x28}, {330, 0x48}, {360, 0x2a}, |
186 | {400, 0x4a}, {450, 0x0c}, {500, 0x2c}, {550, 0x0e}, |
187 | {600, 0x2e}, {650, 0x10}, {700, 0x30}, {750, 0x12}, |
188 | {800, 0x32}, {850, 0x14}, {900, 0x34}, {950, 0x54}, |
189 | {1000, 0x74}, |
190 | }; |
191 | |
192 | static int max_mbps_to_hsfreqrange_sel(u32 max_mbps) |
193 | { |
194 | int i; |
195 | |
196 | for (i = 0; i < ARRAY_SIZE(hsfreq_map); i++) |
197 | if (hsfreq_map[i].max_mbps > max_mbps) |
198 | return hsfreq_map[i].hsfreqrange_sel; |
199 | |
200 | return -EINVAL; |
201 | } |
202 | |
203 | static int csi2_dphy_init(struct csi2_dev *csi2) |
204 | { |
205 | struct v4l2_ctrl *ctrl; |
206 | u32 mbps_per_lane; |
207 | int sel; |
208 | |
209 | ctrl = v4l2_ctrl_find(hdl: csi2->src_sd->ctrl_handler, |
210 | V4L2_CID_LINK_FREQ); |
211 | if (!ctrl) |
212 | mbps_per_lane = CSI2_DEFAULT_MAX_MBPS; |
213 | else |
214 | mbps_per_lane = DIV_ROUND_UP_ULL(2 * ctrl->qmenu_int[ctrl->val], |
215 | USEC_PER_SEC); |
216 | |
217 | sel = max_mbps_to_hsfreqrange_sel(max_mbps: mbps_per_lane); |
218 | if (sel < 0) |
219 | return sel; |
220 | |
221 | dw_mipi_csi2_phy_write(csi2, test_code: 0x44, test_data: sel); |
222 | |
223 | return 0; |
224 | } |
225 | |
226 | /* |
227 | * Waits for ultra-low-power state on D-PHY clock lane. This is currently |
228 | * unused and may not be needed at all, but keep around just in case. |
229 | */ |
230 | static int __maybe_unused csi2_dphy_wait_ulp(struct csi2_dev *csi2) |
231 | { |
232 | u32 reg; |
233 | int ret; |
234 | |
235 | /* wait for ULP on clock lane */ |
236 | ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, |
237 | !(reg & PHY_RXULPSCLKNOT), 0, 500000); |
238 | if (ret) { |
239 | v4l2_err(&csi2->sd, "ULP timeout, phy_state = 0x%08x\n" , reg); |
240 | return ret; |
241 | } |
242 | |
243 | /* wait until no errors on bus */ |
244 | ret = readl_poll_timeout(csi2->base + CSI2_ERR1, reg, |
245 | reg == 0x0, 0, 500000); |
246 | if (ret) { |
247 | v4l2_err(&csi2->sd, "stable bus timeout, err1 = 0x%08x\n" , reg); |
248 | return ret; |
249 | } |
250 | |
251 | return 0; |
252 | } |
253 | |
254 | /* Waits for low-power LP-11 state on data and clock lanes. */ |
255 | static void csi2_dphy_wait_stopstate(struct csi2_dev *csi2, unsigned int lanes) |
256 | { |
257 | u32 mask, reg; |
258 | int ret; |
259 | |
260 | mask = PHY_STOPSTATECLK | (((1 << lanes) - 1) << PHY_STOPSTATEDATA_BIT); |
261 | |
262 | ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, |
263 | (reg & mask) == mask, 0, 500000); |
264 | if (ret) { |
265 | v4l2_warn(&csi2->sd, "LP-11 wait timeout, likely a sensor driver bug, expect capture failures.\n" ); |
266 | v4l2_warn(&csi2->sd, "phy_state = 0x%08x\n" , reg); |
267 | } |
268 | } |
269 | |
270 | /* Wait for active clock on the clock lane. */ |
271 | static int csi2_dphy_wait_clock_lane(struct csi2_dev *csi2) |
272 | { |
273 | u32 reg; |
274 | int ret; |
275 | |
276 | ret = readl_poll_timeout(csi2->base + CSI2_PHY_STATE, reg, |
277 | (reg & PHY_RXCLKACTIVEHS), 0, 500000); |
278 | if (ret) { |
279 | v4l2_err(&csi2->sd, "clock lane timeout, phy_state = 0x%08x\n" , |
280 | reg); |
281 | return ret; |
282 | } |
283 | |
284 | return 0; |
285 | } |
286 | |
287 | /* Setup the i.MX CSI2IPU Gasket */ |
288 | static void csi2ipu_gasket_init(struct csi2_dev *csi2) |
289 | { |
290 | u32 reg = 0; |
291 | |
292 | switch (csi2->format_mbus.code) { |
293 | case MEDIA_BUS_FMT_YUYV8_2X8: |
294 | case MEDIA_BUS_FMT_YUYV8_1X16: |
295 | reg = CSI2IPU_YUV422_YUYV; |
296 | break; |
297 | default: |
298 | break; |
299 | } |
300 | |
301 | writel(val: reg, addr: csi2->base + CSI2IPU_GASKET); |
302 | } |
303 | |
304 | static int csi2_get_active_lanes(struct csi2_dev *csi2, unsigned int *lanes) |
305 | { |
306 | struct v4l2_mbus_config mbus_config = { 0 }; |
307 | int ret; |
308 | |
309 | *lanes = csi2->data_lanes; |
310 | |
311 | ret = v4l2_subdev_call(csi2->remote, pad, get_mbus_config, |
312 | csi2->remote_pad, &mbus_config); |
313 | if (ret == -ENOIOCTLCMD) { |
314 | dev_dbg(csi2->dev, "No remote mbus configuration available\n" ); |
315 | return 0; |
316 | } |
317 | |
318 | if (ret) { |
319 | dev_err(csi2->dev, "Failed to get remote mbus configuration\n" ); |
320 | return ret; |
321 | } |
322 | |
323 | if (mbus_config.type != V4L2_MBUS_CSI2_DPHY) { |
324 | dev_err(csi2->dev, "Unsupported media bus type %u\n" , |
325 | mbus_config.type); |
326 | return -EINVAL; |
327 | } |
328 | |
329 | if (mbus_config.bus.mipi_csi2.num_data_lanes > csi2->data_lanes) { |
330 | dev_err(csi2->dev, |
331 | "Unsupported mbus config: too many data lanes %u\n" , |
332 | mbus_config.bus.mipi_csi2.num_data_lanes); |
333 | return -EINVAL; |
334 | } |
335 | |
336 | *lanes = mbus_config.bus.mipi_csi2.num_data_lanes; |
337 | |
338 | return 0; |
339 | } |
340 | |
341 | static int csi2_start(struct csi2_dev *csi2) |
342 | { |
343 | unsigned int lanes; |
344 | int ret; |
345 | |
346 | ret = clk_prepare_enable(clk: csi2->pix_clk); |
347 | if (ret) |
348 | return ret; |
349 | |
350 | /* setup the gasket */ |
351 | csi2ipu_gasket_init(csi2); |
352 | |
353 | /* Step 3 */ |
354 | ret = csi2_dphy_init(csi2); |
355 | if (ret) |
356 | goto err_disable_clk; |
357 | |
358 | ret = csi2_get_active_lanes(csi2, lanes: &lanes); |
359 | if (ret) |
360 | goto err_disable_clk; |
361 | |
362 | /* Step 4 */ |
363 | csi2_set_lanes(csi2, lanes); |
364 | csi2_enable(csi2, enable: true); |
365 | |
366 | /* Step 5 */ |
367 | ret = v4l2_subdev_call(csi2->src_sd, video, pre_streamon, |
368 | V4L2_SUBDEV_PRE_STREAMON_FL_MANUAL_LP); |
369 | if (ret && ret != -ENOIOCTLCMD) |
370 | goto err_assert_reset; |
371 | csi2_dphy_wait_stopstate(csi2, lanes); |
372 | |
373 | /* Step 6 */ |
374 | ret = v4l2_subdev_call(csi2->src_sd, video, s_stream, 1); |
375 | ret = (ret && ret != -ENOIOCTLCMD) ? ret : 0; |
376 | if (ret) |
377 | goto err_stop_lp11; |
378 | |
379 | /* Step 7 */ |
380 | ret = csi2_dphy_wait_clock_lane(csi2); |
381 | if (ret) |
382 | goto err_stop_upstream; |
383 | |
384 | return 0; |
385 | |
386 | err_stop_upstream: |
387 | v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); |
388 | err_stop_lp11: |
389 | v4l2_subdev_call(csi2->src_sd, video, post_streamoff); |
390 | err_assert_reset: |
391 | csi2_enable(csi2, enable: false); |
392 | err_disable_clk: |
393 | clk_disable_unprepare(clk: csi2->pix_clk); |
394 | return ret; |
395 | } |
396 | |
397 | static void csi2_stop(struct csi2_dev *csi2) |
398 | { |
399 | /* stop upstream */ |
400 | v4l2_subdev_call(csi2->src_sd, video, s_stream, 0); |
401 | v4l2_subdev_call(csi2->src_sd, video, post_streamoff); |
402 | |
403 | csi2_enable(csi2, enable: false); |
404 | clk_disable_unprepare(clk: csi2->pix_clk); |
405 | } |
406 | |
407 | /* |
408 | * V4L2 subdev operations. |
409 | */ |
410 | |
411 | static int csi2_s_stream(struct v4l2_subdev *sd, int enable) |
412 | { |
413 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
414 | int i, ret = 0; |
415 | |
416 | mutex_lock(&csi2->lock); |
417 | |
418 | if (!csi2->src_sd) { |
419 | ret = -EPIPE; |
420 | goto out; |
421 | } |
422 | |
423 | for (i = 0; i < CSI2_NUM_SRC_PADS; i++) { |
424 | if (csi2->sink_linked[i]) |
425 | break; |
426 | } |
427 | if (i >= CSI2_NUM_SRC_PADS) { |
428 | ret = -EPIPE; |
429 | goto out; |
430 | } |
431 | |
432 | /* |
433 | * enable/disable streaming only if stream_count is |
434 | * going from 0 to 1 / 1 to 0. |
435 | */ |
436 | if (csi2->stream_count != !enable) |
437 | goto update_count; |
438 | |
439 | dev_dbg(csi2->dev, "stream %s\n" , enable ? "ON" : "OFF" ); |
440 | if (enable) |
441 | ret = csi2_start(csi2); |
442 | else |
443 | csi2_stop(csi2); |
444 | if (ret) |
445 | goto out; |
446 | |
447 | update_count: |
448 | csi2->stream_count += enable ? 1 : -1; |
449 | if (csi2->stream_count < 0) |
450 | csi2->stream_count = 0; |
451 | out: |
452 | mutex_unlock(lock: &csi2->lock); |
453 | return ret; |
454 | } |
455 | |
456 | static int csi2_link_setup(struct media_entity *entity, |
457 | const struct media_pad *local, |
458 | const struct media_pad *remote, u32 flags) |
459 | { |
460 | struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity); |
461 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
462 | struct v4l2_subdev *remote_sd; |
463 | int ret = 0; |
464 | |
465 | dev_dbg(csi2->dev, "link setup %s -> %s" , remote->entity->name, |
466 | local->entity->name); |
467 | |
468 | remote_sd = media_entity_to_v4l2_subdev(remote->entity); |
469 | |
470 | mutex_lock(&csi2->lock); |
471 | |
472 | if (local->flags & MEDIA_PAD_FL_SOURCE) { |
473 | if (flags & MEDIA_LNK_FL_ENABLED) { |
474 | if (csi2->sink_linked[local->index - 1]) { |
475 | ret = -EBUSY; |
476 | goto out; |
477 | } |
478 | csi2->sink_linked[local->index - 1] = true; |
479 | } else { |
480 | csi2->sink_linked[local->index - 1] = false; |
481 | } |
482 | } else { |
483 | if (flags & MEDIA_LNK_FL_ENABLED) { |
484 | if (csi2->src_sd) { |
485 | ret = -EBUSY; |
486 | goto out; |
487 | } |
488 | csi2->src_sd = remote_sd; |
489 | } else { |
490 | csi2->src_sd = NULL; |
491 | } |
492 | } |
493 | |
494 | out: |
495 | mutex_unlock(lock: &csi2->lock); |
496 | return ret; |
497 | } |
498 | |
499 | static struct v4l2_mbus_framefmt * |
500 | __csi2_get_fmt(struct csi2_dev *csi2, struct v4l2_subdev_state *sd_state, |
501 | unsigned int pad, enum v4l2_subdev_format_whence which) |
502 | { |
503 | if (which == V4L2_SUBDEV_FORMAT_TRY) |
504 | return v4l2_subdev_state_get_format(sd_state, pad); |
505 | else |
506 | return &csi2->format_mbus; |
507 | } |
508 | |
509 | static int csi2_get_fmt(struct v4l2_subdev *sd, |
510 | struct v4l2_subdev_state *sd_state, |
511 | struct v4l2_subdev_format *sdformat) |
512 | { |
513 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
514 | struct v4l2_mbus_framefmt *fmt; |
515 | |
516 | mutex_lock(&csi2->lock); |
517 | |
518 | fmt = __csi2_get_fmt(csi2, sd_state, pad: sdformat->pad, which: sdformat->which); |
519 | |
520 | sdformat->format = *fmt; |
521 | |
522 | mutex_unlock(lock: &csi2->lock); |
523 | |
524 | return 0; |
525 | } |
526 | |
527 | static int csi2_set_fmt(struct v4l2_subdev *sd, |
528 | struct v4l2_subdev_state *sd_state, |
529 | struct v4l2_subdev_format *sdformat) |
530 | { |
531 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
532 | struct v4l2_mbus_framefmt *fmt; |
533 | int ret = 0; |
534 | |
535 | if (sdformat->pad >= CSI2_NUM_PADS) |
536 | return -EINVAL; |
537 | |
538 | mutex_lock(&csi2->lock); |
539 | |
540 | if (csi2->stream_count > 0) { |
541 | ret = -EBUSY; |
542 | goto out; |
543 | } |
544 | |
545 | /* Output pads mirror active input pad, no limits on input pads */ |
546 | if (sdformat->pad != CSI2_SINK_PAD) |
547 | sdformat->format = csi2->format_mbus; |
548 | |
549 | fmt = __csi2_get_fmt(csi2, sd_state, pad: sdformat->pad, which: sdformat->which); |
550 | |
551 | *fmt = sdformat->format; |
552 | out: |
553 | mutex_unlock(lock: &csi2->lock); |
554 | return ret; |
555 | } |
556 | |
557 | static int csi2_registered(struct v4l2_subdev *sd) |
558 | { |
559 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
560 | |
561 | /* set a default mbus format */ |
562 | return imx_media_init_mbus_fmt(mbus: &csi2->format_mbus, |
563 | IMX_MEDIA_DEF_PIX_WIDTH, |
564 | IMX_MEDIA_DEF_PIX_HEIGHT, code: 0, |
565 | field: V4L2_FIELD_NONE, NULL); |
566 | } |
567 | |
568 | /* --------------- CORE OPS --------------- */ |
569 | |
570 | static int csi2_log_status(struct v4l2_subdev *sd) |
571 | { |
572 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
573 | |
574 | v4l2_info(sd, "-----MIPI CSI status-----\n" ); |
575 | v4l2_info(sd, "VERSION: 0x%x\n" , |
576 | readl(csi2->base + CSI2_VERSION)); |
577 | v4l2_info(sd, "N_LANES: 0x%x\n" , |
578 | readl(csi2->base + CSI2_N_LANES)); |
579 | v4l2_info(sd, "PHY_SHUTDOWNZ: 0x%x\n" , |
580 | readl(csi2->base + CSI2_PHY_SHUTDOWNZ)); |
581 | v4l2_info(sd, "DPHY_RSTZ: 0x%x\n" , |
582 | readl(csi2->base + CSI2_DPHY_RSTZ)); |
583 | v4l2_info(sd, "RESETN: 0x%x\n" , |
584 | readl(csi2->base + CSI2_RESETN)); |
585 | v4l2_info(sd, "PHY_STATE: 0x%x\n" , |
586 | readl(csi2->base + CSI2_PHY_STATE)); |
587 | v4l2_info(sd, "DATA_IDS_1: 0x%x\n" , |
588 | readl(csi2->base + CSI2_DATA_IDS_1)); |
589 | v4l2_info(sd, "DATA_IDS_2: 0x%x\n" , |
590 | readl(csi2->base + CSI2_DATA_IDS_2)); |
591 | v4l2_info(sd, "ERR1: 0x%x\n" , |
592 | readl(csi2->base + CSI2_ERR1)); |
593 | v4l2_info(sd, "ERR2: 0x%x\n" , |
594 | readl(csi2->base + CSI2_ERR2)); |
595 | v4l2_info(sd, "MSK1: 0x%x\n" , |
596 | readl(csi2->base + CSI2_MSK1)); |
597 | v4l2_info(sd, "MSK2: 0x%x\n" , |
598 | readl(csi2->base + CSI2_MSK2)); |
599 | v4l2_info(sd, "PHY_TST_CTRL0: 0x%x\n" , |
600 | readl(csi2->base + CSI2_PHY_TST_CTRL0)); |
601 | v4l2_info(sd, "PHY_TST_CTRL1: 0x%x\n" , |
602 | readl(csi2->base + CSI2_PHY_TST_CTRL1)); |
603 | |
604 | return 0; |
605 | } |
606 | |
607 | static const struct v4l2_subdev_core_ops csi2_core_ops = { |
608 | .log_status = csi2_log_status, |
609 | }; |
610 | |
611 | static const struct media_entity_operations csi2_entity_ops = { |
612 | .link_setup = csi2_link_setup, |
613 | .link_validate = v4l2_subdev_link_validate, |
614 | .get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1, |
615 | }; |
616 | |
617 | static const struct v4l2_subdev_video_ops csi2_video_ops = { |
618 | .s_stream = csi2_s_stream, |
619 | }; |
620 | |
621 | static const struct v4l2_subdev_pad_ops csi2_pad_ops = { |
622 | .get_fmt = csi2_get_fmt, |
623 | .set_fmt = csi2_set_fmt, |
624 | }; |
625 | |
626 | static const struct v4l2_subdev_ops csi2_subdev_ops = { |
627 | .core = &csi2_core_ops, |
628 | .video = &csi2_video_ops, |
629 | .pad = &csi2_pad_ops, |
630 | }; |
631 | |
632 | static const struct v4l2_subdev_internal_ops csi2_internal_ops = { |
633 | .init_state = imx_media_init_state, |
634 | .registered = csi2_registered, |
635 | }; |
636 | |
637 | static int csi2_notify_bound(struct v4l2_async_notifier *notifier, |
638 | struct v4l2_subdev *sd, |
639 | struct v4l2_async_connection *asd) |
640 | { |
641 | struct csi2_dev *csi2 = notifier_to_dev(n: notifier); |
642 | struct media_pad *sink = &csi2->sd.entity.pads[CSI2_SINK_PAD]; |
643 | int pad; |
644 | |
645 | pad = media_entity_get_fwnode_pad(entity: &sd->entity, fwnode: asd->match.fwnode, |
646 | MEDIA_PAD_FL_SOURCE); |
647 | if (pad < 0) { |
648 | dev_err(csi2->dev, "Failed to find pad for %s\n" , sd->name); |
649 | return pad; |
650 | } |
651 | |
652 | csi2->remote = sd; |
653 | csi2->remote_pad = pad; |
654 | |
655 | dev_dbg(csi2->dev, "Bound %s pad: %d\n" , sd->name, pad); |
656 | |
657 | return v4l2_create_fwnode_links_to_pad(src_sd: sd, sink, flags: 0); |
658 | } |
659 | |
660 | static void csi2_notify_unbind(struct v4l2_async_notifier *notifier, |
661 | struct v4l2_subdev *sd, |
662 | struct v4l2_async_connection *asd) |
663 | { |
664 | struct csi2_dev *csi2 = notifier_to_dev(n: notifier); |
665 | |
666 | csi2->remote = NULL; |
667 | } |
668 | |
669 | static const struct v4l2_async_notifier_operations csi2_notify_ops = { |
670 | .bound = csi2_notify_bound, |
671 | .unbind = csi2_notify_unbind, |
672 | }; |
673 | |
674 | static int csi2_async_register(struct csi2_dev *csi2) |
675 | { |
676 | struct v4l2_fwnode_endpoint vep = { |
677 | .bus_type = V4L2_MBUS_CSI2_DPHY, |
678 | }; |
679 | struct v4l2_async_connection *asd; |
680 | struct fwnode_handle *ep; |
681 | int ret; |
682 | |
683 | v4l2_async_subdev_nf_init(notifier: &csi2->notifier, sd: &csi2->sd); |
684 | |
685 | ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(csi2->dev), port: 0, endpoint: 0, |
686 | FWNODE_GRAPH_ENDPOINT_NEXT); |
687 | if (!ep) |
688 | return -ENOTCONN; |
689 | |
690 | ret = v4l2_fwnode_endpoint_parse(fwnode: ep, vep: &vep); |
691 | if (ret) |
692 | goto err_parse; |
693 | |
694 | csi2->data_lanes = vep.bus.mipi_csi2.num_data_lanes; |
695 | |
696 | dev_dbg(csi2->dev, "data lanes: %d\n" , vep.bus.mipi_csi2.num_data_lanes); |
697 | dev_dbg(csi2->dev, "flags: 0x%08x\n" , vep.bus.mipi_csi2.flags); |
698 | |
699 | asd = v4l2_async_nf_add_fwnode_remote(&csi2->notifier, ep, |
700 | struct v4l2_async_connection); |
701 | fwnode_handle_put(fwnode: ep); |
702 | |
703 | if (IS_ERR(ptr: asd)) |
704 | return PTR_ERR(ptr: asd); |
705 | |
706 | csi2->notifier.ops = &csi2_notify_ops; |
707 | |
708 | ret = v4l2_async_nf_register(notifier: &csi2->notifier); |
709 | if (ret) |
710 | return ret; |
711 | |
712 | return v4l2_async_register_subdev(sd: &csi2->sd); |
713 | |
714 | err_parse: |
715 | fwnode_handle_put(fwnode: ep); |
716 | return ret; |
717 | } |
718 | |
719 | static int csi2_probe(struct platform_device *pdev) |
720 | { |
721 | struct csi2_dev *csi2; |
722 | struct resource *res; |
723 | int i, ret; |
724 | |
725 | csi2 = devm_kzalloc(dev: &pdev->dev, size: sizeof(*csi2), GFP_KERNEL); |
726 | if (!csi2) |
727 | return -ENOMEM; |
728 | |
729 | csi2->dev = &pdev->dev; |
730 | |
731 | v4l2_subdev_init(sd: &csi2->sd, ops: &csi2_subdev_ops); |
732 | v4l2_set_subdevdata(sd: &csi2->sd, p: &pdev->dev); |
733 | csi2->sd.internal_ops = &csi2_internal_ops; |
734 | csi2->sd.entity.ops = &csi2_entity_ops; |
735 | csi2->sd.dev = &pdev->dev; |
736 | csi2->sd.owner = THIS_MODULE; |
737 | csi2->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE; |
738 | strscpy(csi2->sd.name, DEVICE_NAME, sizeof(csi2->sd.name)); |
739 | csi2->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE; |
740 | csi2->sd.grp_id = IMX_MEDIA_GRP_ID_CSI2; |
741 | |
742 | for (i = 0; i < CSI2_NUM_PADS; i++) { |
743 | csi2->pad[i].flags = (i == CSI2_SINK_PAD) ? |
744 | MEDIA_PAD_FL_SINK : MEDIA_PAD_FL_SOURCE; |
745 | } |
746 | |
747 | ret = media_entity_pads_init(entity: &csi2->sd.entity, CSI2_NUM_PADS, |
748 | pads: csi2->pad); |
749 | if (ret) |
750 | return ret; |
751 | |
752 | csi2->pllref_clk = devm_clk_get(dev: &pdev->dev, id: "ref" ); |
753 | if (IS_ERR(ptr: csi2->pllref_clk)) { |
754 | v4l2_err(&csi2->sd, "failed to get pll reference clock\n" ); |
755 | return PTR_ERR(ptr: csi2->pllref_clk); |
756 | } |
757 | |
758 | csi2->dphy_clk = devm_clk_get(dev: &pdev->dev, id: "dphy" ); |
759 | if (IS_ERR(ptr: csi2->dphy_clk)) { |
760 | v4l2_err(&csi2->sd, "failed to get dphy clock\n" ); |
761 | return PTR_ERR(ptr: csi2->dphy_clk); |
762 | } |
763 | |
764 | csi2->pix_clk = devm_clk_get(dev: &pdev->dev, id: "pix" ); |
765 | if (IS_ERR(ptr: csi2->pix_clk)) { |
766 | v4l2_err(&csi2->sd, "failed to get pixel clock\n" ); |
767 | return PTR_ERR(ptr: csi2->pix_clk); |
768 | } |
769 | |
770 | res = platform_get_resource(pdev, IORESOURCE_MEM, 0); |
771 | if (!res) { |
772 | v4l2_err(&csi2->sd, "failed to get platform resources\n" ); |
773 | return -ENODEV; |
774 | } |
775 | |
776 | csi2->base = devm_ioremap(dev: &pdev->dev, offset: res->start, PAGE_SIZE); |
777 | if (!csi2->base) |
778 | return -ENOMEM; |
779 | |
780 | mutex_init(&csi2->lock); |
781 | |
782 | ret = clk_prepare_enable(clk: csi2->pllref_clk); |
783 | if (ret) { |
784 | v4l2_err(&csi2->sd, "failed to enable pllref_clk\n" ); |
785 | goto rmmutex; |
786 | } |
787 | |
788 | ret = clk_prepare_enable(clk: csi2->dphy_clk); |
789 | if (ret) { |
790 | v4l2_err(&csi2->sd, "failed to enable dphy_clk\n" ); |
791 | goto pllref_off; |
792 | } |
793 | |
794 | platform_set_drvdata(pdev, data: &csi2->sd); |
795 | |
796 | ret = csi2_async_register(csi2); |
797 | if (ret) |
798 | goto clean_notifier; |
799 | |
800 | return 0; |
801 | |
802 | clean_notifier: |
803 | v4l2_async_nf_unregister(notifier: &csi2->notifier); |
804 | v4l2_async_nf_cleanup(notifier: &csi2->notifier); |
805 | clk_disable_unprepare(clk: csi2->dphy_clk); |
806 | pllref_off: |
807 | clk_disable_unprepare(clk: csi2->pllref_clk); |
808 | rmmutex: |
809 | mutex_destroy(lock: &csi2->lock); |
810 | return ret; |
811 | } |
812 | |
813 | static void csi2_remove(struct platform_device *pdev) |
814 | { |
815 | struct v4l2_subdev *sd = platform_get_drvdata(pdev); |
816 | struct csi2_dev *csi2 = sd_to_dev(sdev: sd); |
817 | |
818 | v4l2_async_nf_unregister(notifier: &csi2->notifier); |
819 | v4l2_async_nf_cleanup(notifier: &csi2->notifier); |
820 | v4l2_async_unregister_subdev(sd); |
821 | clk_disable_unprepare(clk: csi2->dphy_clk); |
822 | clk_disable_unprepare(clk: csi2->pllref_clk); |
823 | mutex_destroy(lock: &csi2->lock); |
824 | media_entity_cleanup(entity: &sd->entity); |
825 | } |
826 | |
827 | static const struct of_device_id csi2_dt_ids[] = { |
828 | { .compatible = "fsl,imx6-mipi-csi2" , }, |
829 | { /* sentinel */ } |
830 | }; |
831 | MODULE_DEVICE_TABLE(of, csi2_dt_ids); |
832 | |
833 | static struct platform_driver csi2_driver = { |
834 | .driver = { |
835 | .name = DEVICE_NAME, |
836 | .of_match_table = csi2_dt_ids, |
837 | }, |
838 | .probe = csi2_probe, |
839 | .remove_new = csi2_remove, |
840 | }; |
841 | |
842 | module_platform_driver(csi2_driver); |
843 | |
844 | MODULE_DESCRIPTION("i.MX5/6 MIPI CSI-2 Receiver driver" ); |
845 | MODULE_AUTHOR("Steve Longerbeam <steve_longerbeam@mentor.com>" ); |
846 | MODULE_LICENSE("GPL" ); |
847 | |