1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Driver for the Texas Instruments DS90UB953 video serializer
4 *
5 * Based on a driver from Luca Ceresoli <luca@lucaceresoli.net>
6 *
7 * Copyright (c) 2019 Luca Ceresoli <luca@lucaceresoli.net>
8 * Copyright (c) 2023 Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>
9 */
10
11 #include <linux/clk-provider.h>
12 #include <linux/clk.h>
13 #include <linux/delay.h>
14 #include <linux/gpio/driver.h>
15 #include <linux/i2c-atr.h>
16 #include <linux/i2c.h>
17 #include <linux/kernel.h>
18 #include <linux/math64.h>
19 #include <linux/module.h>
20 #include <linux/property.h>
21 #include <linux/rational.h>
22 #include <linux/regmap.h>
23
24 #include <media/i2c/ds90ub9xx.h>
25 #include <media/v4l2-ctrls.h>
26 #include <media/v4l2-fwnode.h>
27 #include <media/v4l2-mediabus.h>
28 #include <media/v4l2-subdev.h>
29
30 #include "ds90ub953.h"
31
32 #define UB953_PAD_SINK 0
33 #define UB953_PAD_SOURCE 1
34
35 #define UB953_NUM_GPIOS 4
36
37 #define UB953_DEFAULT_CLKOUT_RATE 25000000UL
38
39 /* Note: Only sync mode supported for now */
40 enum ub953_mode {
41 /* FPD-Link III CSI-2 synchronous mode */
42 UB953_MODE_SYNC,
43 /* FPD-Link III CSI-2 non-synchronous mode, external ref clock */
44 UB953_MODE_NONSYNC_EXT,
45 /* FPD-Link III CSI-2 non-synchronous mode, internal ref clock */
46 UB953_MODE_NONSYNC_INT,
47 /* FPD-Link III DVP mode */
48 UB953_MODE_DVP,
49 };
50
51 struct ub953_hw_data {
52 const char *model;
53 bool is_ub971;
54 };
55
56 struct ub953_clkout_data {
57 u32 hs_div;
58 u32 m;
59 u32 n;
60 unsigned long rate;
61 };
62
63 struct ub953_data {
64 const struct ub953_hw_data *hw_data;
65
66 struct i2c_client *client;
67 struct regmap *regmap;
68 struct clk *clkin;
69
70 u32 num_data_lanes;
71 bool non_continous_clk;
72
73 struct gpio_chip gpio_chip;
74
75 struct v4l2_subdev sd;
76 struct media_pad pads[2];
77
78 struct v4l2_async_notifier notifier;
79
80 struct v4l2_subdev *source_sd;
81 u16 source_sd_pad;
82
83 u64 enabled_source_streams;
84
85 /* lock for register access */
86 struct mutex reg_lock;
87
88 u8 current_indirect_target;
89
90 struct clk_hw clkout_clk_hw;
91
92 enum ub953_mode mode;
93
94 const struct ds90ub9xx_platform_data *plat_data;
95 };
96
sd_to_ub953(struct v4l2_subdev * sd)97 static inline struct ub953_data *sd_to_ub953(struct v4l2_subdev *sd)
98 {
99 return container_of(sd, struct ub953_data, sd);
100 }
101
102 /*
103 * HW Access
104 */
105
ub953_read(struct ub953_data * priv,u8 reg,u8 * val,int * err)106 static int ub953_read(struct ub953_data *priv, u8 reg, u8 *val, int *err)
107 {
108 unsigned int v;
109 int ret;
110
111 if (err && *err)
112 return *err;
113
114 mutex_lock(&priv->reg_lock);
115
116 ret = regmap_read(priv->regmap, reg, &v);
117 if (ret) {
118 dev_err(&priv->client->dev, "Cannot read register 0x%02x: %d\n",
119 reg, ret);
120 goto out_unlock;
121 }
122
123 *val = v;
124
125 out_unlock:
126 mutex_unlock(&priv->reg_lock);
127
128 if (ret && err)
129 *err = ret;
130
131 return ret;
132 }
133
ub953_write(struct ub953_data * priv,u8 reg,u8 val,int * err)134 static int ub953_write(struct ub953_data *priv, u8 reg, u8 val, int *err)
135 {
136 int ret;
137
138 if (err && *err)
139 return *err;
140
141 mutex_lock(&priv->reg_lock);
142
143 ret = regmap_write(priv->regmap, reg, val);
144 if (ret)
145 dev_err(&priv->client->dev,
146 "Cannot write register 0x%02x: %d\n", reg, ret);
147
148 mutex_unlock(&priv->reg_lock);
149
150 if (ret && err)
151 *err = ret;
152
153 return ret;
154 }
155
ub953_select_ind_reg_block(struct ub953_data * priv,u8 block)156 static int ub953_select_ind_reg_block(struct ub953_data *priv, u8 block)
157 {
158 struct device *dev = &priv->client->dev;
159 int ret;
160
161 if (priv->current_indirect_target == block)
162 return 0;
163
164 ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_CTL, block << 2);
165 if (ret) {
166 dev_err(dev, "%s: cannot select indirect target %u (%d)\n",
167 __func__, block, ret);
168 return ret;
169 }
170
171 priv->current_indirect_target = block;
172
173 return 0;
174 }
175
176 __maybe_unused
ub953_read_ind(struct ub953_data * priv,u8 block,u8 reg,u8 * val,int * err)177 static int ub953_read_ind(struct ub953_data *priv, u8 block, u8 reg, u8 *val,
178 int *err)
179 {
180 unsigned int v;
181 int ret;
182
183 if (err && *err)
184 return *err;
185
186 mutex_lock(&priv->reg_lock);
187
188 ret = ub953_select_ind_reg_block(priv, block);
189 if (ret)
190 goto out_unlock;
191
192 ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
193 if (ret) {
194 dev_err(&priv->client->dev,
195 "Write to IND_ACC_ADDR failed when reading %u:0x%02x: %d\n",
196 block, reg, ret);
197 goto out_unlock;
198 }
199
200 ret = regmap_read(priv->regmap, UB953_REG_IND_ACC_DATA, &v);
201 if (ret) {
202 dev_err(&priv->client->dev,
203 "Write to IND_ACC_DATA failed when reading %u:0x%02x: %d\n",
204 block, reg, ret);
205 goto out_unlock;
206 }
207
208 *val = v;
209
210 out_unlock:
211 mutex_unlock(&priv->reg_lock);
212
213 if (ret && err)
214 *err = ret;
215
216 return ret;
217 }
218
219 __maybe_unused
ub953_write_ind(struct ub953_data * priv,u8 block,u8 reg,u8 val,int * err)220 static int ub953_write_ind(struct ub953_data *priv, u8 block, u8 reg, u8 val,
221 int *err)
222 {
223 int ret;
224
225 if (err && *err)
226 return *err;
227
228 mutex_lock(&priv->reg_lock);
229
230 ret = ub953_select_ind_reg_block(priv, block);
231 if (ret)
232 goto out_unlock;
233
234 ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_ADDR, reg);
235 if (ret) {
236 dev_err(&priv->client->dev,
237 "Write to IND_ACC_ADDR failed when writing %u:0x%02x: %d\n",
238 block, reg, ret);
239 goto out_unlock;
240 }
241
242 ret = regmap_write(priv->regmap, UB953_REG_IND_ACC_DATA, val);
243 if (ret) {
244 dev_err(&priv->client->dev,
245 "Write to IND_ACC_DATA failed when writing %u:0x%02x: %d\n",
246 block, reg, ret);
247 }
248
249 out_unlock:
250 mutex_unlock(&priv->reg_lock);
251
252 if (ret && err)
253 *err = ret;
254
255 return ret;
256 }
257
258 /*
259 * GPIO chip
260 */
ub953_gpio_get_direction(struct gpio_chip * gc,unsigned int offset)261 static int ub953_gpio_get_direction(struct gpio_chip *gc, unsigned int offset)
262 {
263 struct ub953_data *priv = gpiochip_get_data(gc);
264 int ret;
265 u8 v;
266
267 ret = ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &v, NULL);
268 if (ret)
269 return ret;
270
271 if (v & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset))
272 return GPIO_LINE_DIRECTION_IN;
273 else
274 return GPIO_LINE_DIRECTION_OUT;
275 }
276
ub953_gpio_direction_in(struct gpio_chip * gc,unsigned int offset)277 static int ub953_gpio_direction_in(struct gpio_chip *gc, unsigned int offset)
278 {
279 struct ub953_data *priv = gpiochip_get_data(gc);
280
281 return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
282 UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
283 UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
284 UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset));
285 }
286
ub953_gpio_direction_out(struct gpio_chip * gc,unsigned int offset,int value)287 static int ub953_gpio_direction_out(struct gpio_chip *gc, unsigned int offset,
288 int value)
289 {
290 struct ub953_data *priv = gpiochip_get_data(gc);
291 int ret;
292
293 ret = regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
294 UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
295 value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
296 0);
297
298 if (ret)
299 return ret;
300
301 return regmap_update_bits(priv->regmap, UB953_REG_GPIO_INPUT_CTRL,
302 UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(offset) |
303 UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset),
304 UB953_REG_GPIO_INPUT_CTRL_OUT_EN(offset));
305 }
306
ub953_gpio_get(struct gpio_chip * gc,unsigned int offset)307 static int ub953_gpio_get(struct gpio_chip *gc, unsigned int offset)
308 {
309 struct ub953_data *priv = gpiochip_get_data(gc);
310 int ret;
311 u8 v;
312
313 ret = ub953_read(priv, UB953_REG_GPIO_PIN_STS, &v, NULL);
314 if (ret)
315 return ret;
316
317 return !!(v & UB953_REG_GPIO_PIN_STS_GPIO_STS(offset));
318 }
319
ub953_gpio_set(struct gpio_chip * gc,unsigned int offset,int value)320 static void ub953_gpio_set(struct gpio_chip *gc, unsigned int offset, int value)
321 {
322 struct ub953_data *priv = gpiochip_get_data(gc);
323
324 regmap_update_bits(priv->regmap, UB953_REG_LOCAL_GPIO_DATA,
325 UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset),
326 value ? UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(offset) :
327 0);
328 }
329
ub953_gpio_of_xlate(struct gpio_chip * gc,const struct of_phandle_args * gpiospec,u32 * flags)330 static int ub953_gpio_of_xlate(struct gpio_chip *gc,
331 const struct of_phandle_args *gpiospec,
332 u32 *flags)
333 {
334 if (flags)
335 *flags = gpiospec->args[1];
336
337 return gpiospec->args[0];
338 }
339
ub953_gpiochip_probe(struct ub953_data * priv)340 static int ub953_gpiochip_probe(struct ub953_data *priv)
341 {
342 struct device *dev = &priv->client->dev;
343 struct gpio_chip *gc = &priv->gpio_chip;
344 int ret;
345
346 /* Set all GPIOs to local input mode */
347 ret = ub953_write(priv, UB953_REG_LOCAL_GPIO_DATA, 0, NULL);
348 if (ret)
349 return ret;
350
351 ret = ub953_write(priv, UB953_REG_GPIO_INPUT_CTRL, 0xf, NULL);
352 if (ret)
353 return ret;
354
355 gc->label = dev_name(dev);
356 gc->parent = dev;
357 gc->owner = THIS_MODULE;
358 gc->base = -1;
359 gc->can_sleep = true;
360 gc->ngpio = UB953_NUM_GPIOS;
361 gc->get_direction = ub953_gpio_get_direction;
362 gc->direction_input = ub953_gpio_direction_in;
363 gc->direction_output = ub953_gpio_direction_out;
364 gc->get = ub953_gpio_get;
365 gc->set = ub953_gpio_set;
366 gc->of_xlate = ub953_gpio_of_xlate;
367 gc->of_gpio_n_cells = 2;
368
369 ret = gpiochip_add_data(gc, priv);
370 if (ret) {
371 dev_err(dev, "Failed to add GPIOs: %d\n", ret);
372 return ret;
373 }
374
375 return 0;
376 }
377
ub953_gpiochip_remove(struct ub953_data * priv)378 static void ub953_gpiochip_remove(struct ub953_data *priv)
379 {
380 gpiochip_remove(&priv->gpio_chip);
381 }
382
383 /*
384 * V4L2
385 */
386
_ub953_set_routing(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_krouting * routing)387 static int _ub953_set_routing(struct v4l2_subdev *sd,
388 struct v4l2_subdev_state *state,
389 struct v4l2_subdev_krouting *routing)
390 {
391 static const struct v4l2_mbus_framefmt format = {
392 .width = 640,
393 .height = 480,
394 .code = MEDIA_BUS_FMT_UYVY8_1X16,
395 .field = V4L2_FIELD_NONE,
396 .colorspace = V4L2_COLORSPACE_SRGB,
397 .ycbcr_enc = V4L2_YCBCR_ENC_601,
398 .quantization = V4L2_QUANTIZATION_LIM_RANGE,
399 .xfer_func = V4L2_XFER_FUNC_SRGB,
400 };
401 int ret;
402
403 /*
404 * Note: we can only support up to V4L2_FRAME_DESC_ENTRY_MAX, until
405 * frame desc is made dynamically allocated.
406 */
407
408 if (routing->num_routes > V4L2_FRAME_DESC_ENTRY_MAX)
409 return -EINVAL;
410
411 ret = v4l2_subdev_routing_validate(sd, routing,
412 V4L2_SUBDEV_ROUTING_ONLY_1_TO_1);
413 if (ret)
414 return ret;
415
416 ret = v4l2_subdev_set_routing_with_fmt(sd, state, routing, &format);
417 if (ret)
418 return ret;
419
420 return 0;
421 }
422
ub953_set_routing(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,enum v4l2_subdev_format_whence which,struct v4l2_subdev_krouting * routing)423 static int ub953_set_routing(struct v4l2_subdev *sd,
424 struct v4l2_subdev_state *state,
425 enum v4l2_subdev_format_whence which,
426 struct v4l2_subdev_krouting *routing)
427 {
428 struct ub953_data *priv = sd_to_ub953(sd);
429
430 if (which == V4L2_SUBDEV_FORMAT_ACTIVE && priv->enabled_source_streams)
431 return -EBUSY;
432
433 return _ub953_set_routing(sd, state, routing);
434 }
435
ub953_get_frame_desc(struct v4l2_subdev * sd,unsigned int pad,struct v4l2_mbus_frame_desc * fd)436 static int ub953_get_frame_desc(struct v4l2_subdev *sd, unsigned int pad,
437 struct v4l2_mbus_frame_desc *fd)
438 {
439 struct ub953_data *priv = sd_to_ub953(sd);
440 struct v4l2_mbus_frame_desc source_fd;
441 struct v4l2_subdev_route *route;
442 struct v4l2_subdev_state *state;
443 int ret;
444
445 if (pad != UB953_PAD_SOURCE)
446 return -EINVAL;
447
448 ret = v4l2_subdev_call(priv->source_sd, pad, get_frame_desc,
449 priv->source_sd_pad, &source_fd);
450 if (ret)
451 return ret;
452
453 fd->type = V4L2_MBUS_FRAME_DESC_TYPE_CSI2;
454
455 state = v4l2_subdev_lock_and_get_active_state(sd);
456
457 for_each_active_route(&state->routing, route) {
458 struct v4l2_mbus_frame_desc_entry *source_entry = NULL;
459 unsigned int i;
460
461 if (route->source_pad != pad)
462 continue;
463
464 for (i = 0; i < source_fd.num_entries; i++) {
465 if (source_fd.entry[i].stream == route->sink_stream) {
466 source_entry = &source_fd.entry[i];
467 break;
468 }
469 }
470
471 if (!source_entry) {
472 dev_err(&priv->client->dev,
473 "Failed to find stream from source frame desc\n");
474 ret = -EPIPE;
475 goto out_unlock;
476 }
477
478 fd->entry[fd->num_entries].stream = route->source_stream;
479 fd->entry[fd->num_entries].flags = source_entry->flags;
480 fd->entry[fd->num_entries].length = source_entry->length;
481 fd->entry[fd->num_entries].pixelcode = source_entry->pixelcode;
482 fd->entry[fd->num_entries].bus.csi2.vc =
483 source_entry->bus.csi2.vc;
484 fd->entry[fd->num_entries].bus.csi2.dt =
485 source_entry->bus.csi2.dt;
486
487 fd->num_entries++;
488 }
489
490 out_unlock:
491 v4l2_subdev_unlock_state(state);
492
493 return ret;
494 }
495
ub953_set_fmt(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,struct v4l2_subdev_format * format)496 static int ub953_set_fmt(struct v4l2_subdev *sd,
497 struct v4l2_subdev_state *state,
498 struct v4l2_subdev_format *format)
499 {
500 struct ub953_data *priv = sd_to_ub953(sd);
501 struct v4l2_mbus_framefmt *fmt;
502
503 if (format->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
504 priv->enabled_source_streams)
505 return -EBUSY;
506
507 /* No transcoding, source and sink formats must match. */
508 if (format->pad == UB953_PAD_SOURCE)
509 return v4l2_subdev_get_fmt(sd, state, format);
510
511 /* Set sink format */
512 fmt = v4l2_subdev_state_get_format(state, format->pad, format->stream);
513 if (!fmt)
514 return -EINVAL;
515
516 *fmt = format->format;
517
518 /* Propagate to source format */
519 fmt = v4l2_subdev_state_get_opposite_stream_format(state, format->pad,
520 format->stream);
521 if (!fmt)
522 return -EINVAL;
523
524 *fmt = format->format;
525
526 return 0;
527 }
528
ub953_init_state(struct v4l2_subdev * sd,struct v4l2_subdev_state * state)529 static int ub953_init_state(struct v4l2_subdev *sd,
530 struct v4l2_subdev_state *state)
531 {
532 struct v4l2_subdev_route routes[] = {
533 {
534 .sink_pad = UB953_PAD_SINK,
535 .sink_stream = 0,
536 .source_pad = UB953_PAD_SOURCE,
537 .source_stream = 0,
538 .flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE,
539 },
540 };
541
542 struct v4l2_subdev_krouting routing = {
543 .num_routes = ARRAY_SIZE(routes),
544 .routes = routes,
545 };
546
547 return _ub953_set_routing(sd, state, &routing);
548 }
549
ub953_log_status(struct v4l2_subdev * sd)550 static int ub953_log_status(struct v4l2_subdev *sd)
551 {
552 struct ub953_data *priv = sd_to_ub953(sd);
553 struct device *dev = &priv->client->dev;
554 char id[UB953_REG_FPD3_RX_ID_LEN];
555 u8 gpio_local_data;
556 u8 gpio_input_ctrl;
557 u8 gpio_pin_sts;
558 unsigned int i;
559 u8 v, v1, v2;
560 int ret;
561
562 for (i = 0; i < sizeof(id); i++) {
563 ret = ub953_read(priv, UB953_REG_FPD3_RX_ID(i), &id[i], NULL);
564 if (ret)
565 return ret;
566 }
567
568 dev_info(dev, "ID '%.*s'\n", (int)sizeof(id), id);
569
570 ret = ub953_read(priv, UB953_REG_GENERAL_STATUS, &v, NULL);
571 if (ret)
572 return ret;
573
574 dev_info(dev, "GENERAL_STATUS %#02x\n", v);
575
576 ub953_read(priv, UB953_REG_CRC_ERR_CNT1, &v1, &ret);
577 ub953_read(priv, UB953_REG_CRC_ERR_CNT2, &v2, &ret);
578 if (ret)
579 return ret;
580
581 dev_info(dev, "CRC error count %u\n", v1 | (v2 << 8));
582
583 /* Clear CRC error counter */
584 if (v1 || v2)
585 regmap_update_bits(priv->regmap, UB953_REG_BC_CTRL,
586 UB953_REG_BC_CTRL_CRC_ERR_CLR,
587 UB953_REG_BC_CTRL_CRC_ERR_CLR);
588
589 ret = ub953_read(priv, UB953_REG_CSI_ERR_CNT, &v, NULL);
590 if (ret)
591 return ret;
592
593 dev_info(dev, "CSI error count %u\n", v);
594
595 ret = ub953_read(priv, UB953_REG_CSI_ERR_STATUS, &v, NULL);
596 if (ret)
597 return ret;
598
599 dev_info(dev, "CSI_ERR_STATUS %#02x\n", v);
600
601 ret = ub953_read(priv, UB953_REG_CSI_ERR_DLANE01, &v, NULL);
602 if (ret)
603 return ret;
604
605 dev_info(dev, "CSI_ERR_DLANE01 %#02x\n", v);
606
607 ret = ub953_read(priv, UB953_REG_CSI_ERR_DLANE23, &v, NULL);
608 if (ret)
609 return ret;
610
611 dev_info(dev, "CSI_ERR_DLANE23 %#02x\n", v);
612
613 ret = ub953_read(priv, UB953_REG_CSI_ERR_CLK_LANE, &v, NULL);
614 if (ret)
615 return ret;
616
617 dev_info(dev, "CSI_ERR_CLK_LANE %#02x\n", v);
618
619 ret = ub953_read(priv, UB953_REG_CSI_PKT_HDR_VC_ID, &v, NULL);
620 if (ret)
621 return ret;
622
623 dev_info(dev, "CSI packet header VC %u ID %u\n", v >> 6, v & 0x3f);
624
625 ub953_read(priv, UB953_REG_PKT_HDR_WC_LSB, &v1, &ret);
626 ub953_read(priv, UB953_REG_PKT_HDR_WC_MSB, &v2, &ret);
627 if (ret)
628 return ret;
629
630 dev_info(dev, "CSI packet header WC %u\n", (v2 << 8) | v1);
631
632 ret = ub953_read(priv, UB953_REG_CSI_ECC, &v, NULL);
633 if (ret)
634 return ret;
635
636 dev_info(dev, "CSI ECC %#02x\n", v);
637
638 ub953_read(priv, UB953_REG_LOCAL_GPIO_DATA, &gpio_local_data, &ret);
639 ub953_read(priv, UB953_REG_GPIO_INPUT_CTRL, &gpio_input_ctrl, &ret);
640 ub953_read(priv, UB953_REG_GPIO_PIN_STS, &gpio_pin_sts, &ret);
641 if (ret)
642 return ret;
643
644 for (i = 0; i < UB953_NUM_GPIOS; i++) {
645 dev_info(dev,
646 "GPIO%u: remote: %u is_input: %u is_output: %u val: %u sts: %u\n",
647 i,
648 !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_RMTEN(i)),
649 !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_INPUT_EN(i)),
650 !!(gpio_input_ctrl & UB953_REG_GPIO_INPUT_CTRL_OUT_EN(i)),
651 !!(gpio_local_data & UB953_REG_LOCAL_GPIO_DATA_GPIO_OUT_SRC(i)),
652 !!(gpio_pin_sts & UB953_REG_GPIO_PIN_STS_GPIO_STS(i)));
653 }
654
655 return 0;
656 }
657
ub953_enable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)658 static int ub953_enable_streams(struct v4l2_subdev *sd,
659 struct v4l2_subdev_state *state, u32 pad,
660 u64 streams_mask)
661 {
662 struct ub953_data *priv = sd_to_ub953(sd);
663 u64 sink_streams;
664 int ret;
665
666 sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
667 UB953_PAD_SINK,
668 &streams_mask);
669
670 ret = v4l2_subdev_enable_streams(priv->source_sd, priv->source_sd_pad,
671 sink_streams);
672 if (ret)
673 return ret;
674
675 priv->enabled_source_streams |= streams_mask;
676
677 return 0;
678 }
679
ub953_disable_streams(struct v4l2_subdev * sd,struct v4l2_subdev_state * state,u32 pad,u64 streams_mask)680 static int ub953_disable_streams(struct v4l2_subdev *sd,
681 struct v4l2_subdev_state *state, u32 pad,
682 u64 streams_mask)
683 {
684 struct ub953_data *priv = sd_to_ub953(sd);
685 u64 sink_streams;
686 int ret;
687
688 sink_streams = v4l2_subdev_state_xlate_streams(state, UB953_PAD_SOURCE,
689 UB953_PAD_SINK,
690 &streams_mask);
691
692 ret = v4l2_subdev_disable_streams(priv->source_sd, priv->source_sd_pad,
693 sink_streams);
694 if (ret)
695 return ret;
696
697 priv->enabled_source_streams &= ~streams_mask;
698
699 return 0;
700 }
701
702 static const struct v4l2_subdev_pad_ops ub953_pad_ops = {
703 .enable_streams = ub953_enable_streams,
704 .disable_streams = ub953_disable_streams,
705 .set_routing = ub953_set_routing,
706 .get_frame_desc = ub953_get_frame_desc,
707 .get_fmt = v4l2_subdev_get_fmt,
708 .set_fmt = ub953_set_fmt,
709 };
710
711 static const struct v4l2_subdev_core_ops ub953_subdev_core_ops = {
712 .log_status = ub953_log_status,
713 };
714
715 static const struct v4l2_subdev_ops ub953_subdev_ops = {
716 .core = &ub953_subdev_core_ops,
717 .pad = &ub953_pad_ops,
718 };
719
720 static const struct v4l2_subdev_internal_ops ub953_internal_ops = {
721 .init_state = ub953_init_state,
722 };
723
724 static const struct media_entity_operations ub953_entity_ops = {
725 .link_validate = v4l2_subdev_link_validate,
726 };
727
ub953_notify_bound(struct v4l2_async_notifier * notifier,struct v4l2_subdev * source_subdev,struct v4l2_async_connection * asd)728 static int ub953_notify_bound(struct v4l2_async_notifier *notifier,
729 struct v4l2_subdev *source_subdev,
730 struct v4l2_async_connection *asd)
731 {
732 struct ub953_data *priv = sd_to_ub953(notifier->sd);
733 struct device *dev = &priv->client->dev;
734 int ret;
735
736 ret = media_entity_get_fwnode_pad(&source_subdev->entity,
737 source_subdev->fwnode,
738 MEDIA_PAD_FL_SOURCE);
739 if (ret < 0) {
740 dev_err(dev, "Failed to find pad for %s\n",
741 source_subdev->name);
742 return ret;
743 }
744
745 priv->source_sd = source_subdev;
746 priv->source_sd_pad = ret;
747
748 ret = media_create_pad_link(&source_subdev->entity, priv->source_sd_pad,
749 &priv->sd.entity, 0,
750 MEDIA_LNK_FL_ENABLED |
751 MEDIA_LNK_FL_IMMUTABLE);
752 if (ret) {
753 dev_err(dev, "Unable to link %s:%u -> %s:0\n",
754 source_subdev->name, priv->source_sd_pad,
755 priv->sd.name);
756 return ret;
757 }
758
759 return 0;
760 }
761
762 static const struct v4l2_async_notifier_operations ub953_notify_ops = {
763 .bound = ub953_notify_bound,
764 };
765
ub953_v4l2_notifier_register(struct ub953_data * priv)766 static int ub953_v4l2_notifier_register(struct ub953_data *priv)
767 {
768 struct device *dev = &priv->client->dev;
769 struct v4l2_async_connection *asd;
770 struct fwnode_handle *ep_fwnode;
771 int ret;
772
773 ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
774 UB953_PAD_SINK, 0, 0);
775 if (!ep_fwnode) {
776 dev_err(dev, "No graph endpoint\n");
777 return -ENODEV;
778 }
779
780 v4l2_async_subdev_nf_init(&priv->notifier, &priv->sd);
781
782 asd = v4l2_async_nf_add_fwnode_remote(&priv->notifier, ep_fwnode,
783 struct v4l2_async_connection);
784
785 fwnode_handle_put(ep_fwnode);
786
787 if (IS_ERR(asd)) {
788 dev_err(dev, "Failed to add subdev: %ld", PTR_ERR(asd));
789 v4l2_async_nf_cleanup(&priv->notifier);
790 return PTR_ERR(asd);
791 }
792
793 priv->notifier.ops = &ub953_notify_ops;
794
795 ret = v4l2_async_nf_register(&priv->notifier);
796 if (ret) {
797 dev_err(dev, "Failed to register subdev_notifier");
798 v4l2_async_nf_cleanup(&priv->notifier);
799 return ret;
800 }
801
802 return 0;
803 }
804
ub953_v4l2_notifier_unregister(struct ub953_data * priv)805 static void ub953_v4l2_notifier_unregister(struct ub953_data *priv)
806 {
807 v4l2_async_nf_unregister(&priv->notifier);
808 v4l2_async_nf_cleanup(&priv->notifier);
809 }
810
811 /*
812 * Probing
813 */
814
ub953_i2c_master_init(struct ub953_data * priv)815 static int ub953_i2c_master_init(struct ub953_data *priv)
816 {
817 /* i2c fast mode */
818 u32 ref = 26250000;
819 u32 scl_high = 915; /* ns */
820 u32 scl_low = 1641; /* ns */
821 int ret;
822
823 scl_high = div64_u64((u64)scl_high * ref, 1000000000) - 5;
824 scl_low = div64_u64((u64)scl_low * ref, 1000000000) - 5;
825
826 ret = ub953_write(priv, UB953_REG_SCL_HIGH_TIME, scl_high, NULL);
827 if (ret)
828 return ret;
829
830 ret = ub953_write(priv, UB953_REG_SCL_LOW_TIME, scl_low, NULL);
831 if (ret)
832 return ret;
833
834 return 0;
835 }
836
ub953_get_fc_rate(struct ub953_data * priv)837 static u64 ub953_get_fc_rate(struct ub953_data *priv)
838 {
839 switch (priv->mode) {
840 case UB953_MODE_SYNC:
841 if (priv->hw_data->is_ub971)
842 return priv->plat_data->bc_rate * 160ull;
843 else
844 return priv->plat_data->bc_rate / 2 * 160ull;
845
846 case UB953_MODE_NONSYNC_EXT:
847 /* CLKIN_DIV = 1 always */
848 return clk_get_rate(priv->clkin) * 80ull;
849
850 default:
851 /* Not supported */
852 return 0;
853 }
854 }
855
ub953_calc_clkout_ub953(struct ub953_data * priv,unsigned long target,u64 fc,u8 * hs_div,u8 * m,u8 * n)856 static unsigned long ub953_calc_clkout_ub953(struct ub953_data *priv,
857 unsigned long target, u64 fc,
858 u8 *hs_div, u8 *m, u8 *n)
859 {
860 /*
861 * We always use 4 as a pre-divider (HS_CLK_DIV = 2).
862 *
863 * According to the datasheet:
864 * - "HS_CLK_DIV typically should be set to either 16, 8, or 4 (default)."
865 * - "if it is not possible to have an integer ratio of N/M, it is best to
866 * select a smaller value for HS_CLK_DIV.
867 *
868 * For above reasons the default HS_CLK_DIV seems the best in the average
869 * case. Use always that value to keep the code simple.
870 */
871 static const unsigned long hs_clk_div = 4;
872
873 u64 fc_divided;
874 unsigned long mul, div;
875 unsigned long res;
876
877 /* clkout = fc / hs_clk_div * m / n */
878
879 fc_divided = div_u64(fc, hs_clk_div);
880
881 rational_best_approximation(target, fc_divided, (1 << 5) - 1,
882 (1 << 8) - 1, &mul, &div);
883
884 res = div_u64(fc_divided * mul, div);
885
886 *hs_div = hs_clk_div;
887 *m = mul;
888 *n = div;
889
890 return res;
891 }
892
ub953_calc_clkout_ub971(struct ub953_data * priv,unsigned long target,u64 fc,u8 * m,u8 * n)893 static unsigned long ub953_calc_clkout_ub971(struct ub953_data *priv,
894 unsigned long target, u64 fc,
895 u8 *m, u8 *n)
896 {
897 u64 fc_divided;
898 unsigned long mul, div;
899 unsigned long res;
900
901 /* clkout = fc * m / (8 * n) */
902
903 fc_divided = div_u64(fc, 8);
904
905 rational_best_approximation(target, fc_divided, (1 << 5) - 1,
906 (1 << 8) - 1, &mul, &div);
907
908 res = div_u64(fc_divided * mul, div);
909
910 *m = mul;
911 *n = div;
912
913 return res;
914 }
915
ub953_calc_clkout_params(struct ub953_data * priv,unsigned long target_rate,struct ub953_clkout_data * clkout_data)916 static void ub953_calc_clkout_params(struct ub953_data *priv,
917 unsigned long target_rate,
918 struct ub953_clkout_data *clkout_data)
919 {
920 struct device *dev = &priv->client->dev;
921 unsigned long clkout_rate;
922 u64 fc_rate;
923
924 fc_rate = ub953_get_fc_rate(priv);
925
926 if (priv->hw_data->is_ub971) {
927 u8 m, n;
928
929 clkout_rate = ub953_calc_clkout_ub971(priv, target_rate,
930 fc_rate, &m, &n);
931
932 clkout_data->m = m;
933 clkout_data->n = n;
934
935 dev_dbg(dev, "%s %llu * %u / (8 * %u) = %lu (requested %lu)",
936 __func__, fc_rate, m, n, clkout_rate, target_rate);
937 } else {
938 u8 hs_div, m, n;
939
940 clkout_rate = ub953_calc_clkout_ub953(priv, target_rate,
941 fc_rate, &hs_div, &m, &n);
942
943 clkout_data->hs_div = hs_div;
944 clkout_data->m = m;
945 clkout_data->n = n;
946
947 dev_dbg(dev, "%s %llu / %u * %u / %u = %lu (requested %lu)",
948 __func__, fc_rate, hs_div, m, n, clkout_rate,
949 target_rate);
950 }
951
952 clkout_data->rate = clkout_rate;
953 }
954
ub953_write_clkout_regs(struct ub953_data * priv,const struct ub953_clkout_data * clkout_data)955 static int ub953_write_clkout_regs(struct ub953_data *priv,
956 const struct ub953_clkout_data *clkout_data)
957 {
958 u8 clkout_ctrl0, clkout_ctrl1;
959 int ret;
960
961 if (priv->hw_data->is_ub971)
962 clkout_ctrl0 = clkout_data->m;
963 else
964 clkout_ctrl0 = (__ffs(clkout_data->hs_div) << 5) |
965 clkout_data->m;
966
967 clkout_ctrl1 = clkout_data->n;
968
969 ret = ub953_write(priv, UB953_REG_CLKOUT_CTRL0, clkout_ctrl0, NULL);
970 if (ret)
971 return ret;
972
973 ret = ub953_write(priv, UB953_REG_CLKOUT_CTRL1, clkout_ctrl1, NULL);
974 if (ret)
975 return ret;
976
977 return 0;
978 }
979
ub953_clkout_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)980 static unsigned long ub953_clkout_recalc_rate(struct clk_hw *hw,
981 unsigned long parent_rate)
982 {
983 struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
984 struct device *dev = &priv->client->dev;
985 u8 ctrl0, ctrl1;
986 u32 mul, div;
987 u64 fc_rate;
988 u32 hs_clk_div;
989 u64 rate;
990 int ret;
991
992 ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL0, &ctrl0, NULL);
993 if (ret) {
994 dev_err(dev, "Failed to read CLKOUT_CTRL0: %d\n", ret);
995 return 0;
996 }
997
998 ret = ub953_read(priv, UB953_REG_CLKOUT_CTRL1, &ctrl1, NULL);
999 if (ret) {
1000 dev_err(dev, "Failed to read CLKOUT_CTRL1: %d\n", ret);
1001 return 0;
1002 }
1003
1004 fc_rate = ub953_get_fc_rate(priv);
1005
1006 if (priv->hw_data->is_ub971) {
1007 mul = ctrl0 & 0x1f;
1008 div = ctrl1;
1009
1010 if (div == 0)
1011 return 0;
1012
1013 rate = div_u64(fc_rate * mul, 8 * div);
1014
1015 dev_dbg(dev, "clkout: fc rate %llu, mul %u, div %u = %llu\n",
1016 fc_rate, mul, div, rate);
1017 } else {
1018 mul = ctrl0 & 0x1f;
1019 hs_clk_div = 1 << (ctrl0 >> 5);
1020 div = ctrl1;
1021
1022 if (div == 0)
1023 return 0;
1024
1025 rate = div_u64(div_u64(fc_rate, hs_clk_div) * mul, div);
1026
1027 dev_dbg(dev,
1028 "clkout: fc rate %llu, hs_clk_div %u, mul %u, div %u = %llu\n",
1029 fc_rate, hs_clk_div, mul, div, rate);
1030 }
1031
1032 return rate;
1033 }
1034
ub953_clkout_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)1035 static long ub953_clkout_round_rate(struct clk_hw *hw, unsigned long rate,
1036 unsigned long *parent_rate)
1037 {
1038 struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
1039 struct ub953_clkout_data clkout_data;
1040
1041 ub953_calc_clkout_params(priv, rate, &clkout_data);
1042
1043 return clkout_data.rate;
1044 }
1045
ub953_clkout_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)1046 static int ub953_clkout_set_rate(struct clk_hw *hw, unsigned long rate,
1047 unsigned long parent_rate)
1048 {
1049 struct ub953_data *priv = container_of(hw, struct ub953_data, clkout_clk_hw);
1050 struct ub953_clkout_data clkout_data;
1051
1052 ub953_calc_clkout_params(priv, rate, &clkout_data);
1053
1054 dev_dbg(&priv->client->dev, "%s %lu (requested %lu)\n", __func__,
1055 clkout_data.rate, rate);
1056
1057 return ub953_write_clkout_regs(priv, &clkout_data);
1058 }
1059
1060 static const struct clk_ops ub953_clkout_ops = {
1061 .recalc_rate = ub953_clkout_recalc_rate,
1062 .round_rate = ub953_clkout_round_rate,
1063 .set_rate = ub953_clkout_set_rate,
1064 };
1065
ub953_register_clkout(struct ub953_data * priv)1066 static int ub953_register_clkout(struct ub953_data *priv)
1067 {
1068 struct device *dev = &priv->client->dev;
1069 const struct clk_init_data init = {
1070 .name = kasprintf(GFP_KERNEL, "ds90%s.%s.clk_out",
1071 priv->hw_data->model, dev_name(dev)),
1072 .ops = &ub953_clkout_ops,
1073 };
1074 struct ub953_clkout_data clkout_data;
1075 int ret;
1076
1077 if (!init.name)
1078 return -ENOMEM;
1079
1080 /* Initialize clkout to 25MHz by default */
1081 ub953_calc_clkout_params(priv, UB953_DEFAULT_CLKOUT_RATE, &clkout_data);
1082 ret = ub953_write_clkout_regs(priv, &clkout_data);
1083 if (ret)
1084 return ret;
1085
1086 priv->clkout_clk_hw.init = &init;
1087
1088 ret = devm_clk_hw_register(dev, &priv->clkout_clk_hw);
1089 kfree(init.name);
1090 if (ret)
1091 return dev_err_probe(dev, ret, "Cannot register clock HW\n");
1092
1093 ret = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1094 &priv->clkout_clk_hw);
1095 if (ret)
1096 return dev_err_probe(dev, ret,
1097 "Cannot add OF clock provider\n");
1098
1099 return 0;
1100 }
1101
ub953_add_i2c_adapter(struct ub953_data * priv)1102 static int ub953_add_i2c_adapter(struct ub953_data *priv)
1103 {
1104 struct device *dev = &priv->client->dev;
1105 struct i2c_atr_adap_desc desc = { };
1106 struct fwnode_handle *i2c_handle;
1107 int ret;
1108
1109 i2c_handle = device_get_named_child_node(dev, "i2c");
1110 if (!i2c_handle)
1111 return 0;
1112
1113 desc.chan_id = priv->plat_data->port;
1114 desc.parent = dev;
1115 desc.bus_handle = i2c_handle;
1116 desc.num_aliases = 0;
1117
1118 ret = i2c_atr_add_adapter(priv->plat_data->atr, &desc);
1119
1120 fwnode_handle_put(i2c_handle);
1121
1122 if (ret)
1123 return ret;
1124
1125 return 0;
1126 }
1127
1128 static const struct regmap_config ub953_regmap_config = {
1129 .name = "ds90ub953",
1130 .reg_bits = 8,
1131 .val_bits = 8,
1132 .reg_format_endian = REGMAP_ENDIAN_DEFAULT,
1133 .val_format_endian = REGMAP_ENDIAN_DEFAULT,
1134 };
1135
ub953_parse_dt(struct ub953_data * priv)1136 static int ub953_parse_dt(struct ub953_data *priv)
1137 {
1138 struct device *dev = &priv->client->dev;
1139 struct v4l2_fwnode_endpoint vep = {
1140 .bus_type = V4L2_MBUS_CSI2_DPHY,
1141 };
1142 struct fwnode_handle *ep_fwnode;
1143 unsigned char nlanes;
1144 int ret;
1145
1146 ep_fwnode = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev),
1147 UB953_PAD_SINK, 0, 0);
1148 if (!ep_fwnode)
1149 return dev_err_probe(dev, -ENOENT, "no endpoint found\n");
1150
1151 ret = v4l2_fwnode_endpoint_parse(ep_fwnode, &vep);
1152
1153 fwnode_handle_put(ep_fwnode);
1154
1155 if (ret)
1156 return dev_err_probe(dev, ret,
1157 "failed to parse sink endpoint data\n");
1158
1159 nlanes = vep.bus.mipi_csi2.num_data_lanes;
1160 if (nlanes != 1 && nlanes != 2 && nlanes != 4)
1161 return dev_err_probe(dev, -EINVAL,
1162 "bad number of data-lanes: %u\n", nlanes);
1163
1164 priv->num_data_lanes = nlanes;
1165
1166 priv->non_continous_clk = vep.bus.mipi_csi2.flags &
1167 V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK;
1168
1169 return 0;
1170 }
1171
ub953_hw_init(struct ub953_data * priv)1172 static int ub953_hw_init(struct ub953_data *priv)
1173 {
1174 struct device *dev = &priv->client->dev;
1175 bool mode_override;
1176 int ret;
1177 u8 v;
1178
1179 ret = ub953_read(priv, UB953_REG_MODE_SEL, &v, NULL);
1180 if (ret)
1181 return ret;
1182
1183 if (!(v & UB953_REG_MODE_SEL_MODE_DONE))
1184 return dev_err_probe(dev, -EIO, "Mode value not stabilized\n");
1185
1186 mode_override = v & UB953_REG_MODE_SEL_MODE_OVERRIDE;
1187
1188 switch (v & UB953_REG_MODE_SEL_MODE_MASK) {
1189 case 0:
1190 priv->mode = UB953_MODE_SYNC;
1191 break;
1192 case 2:
1193 priv->mode = UB953_MODE_NONSYNC_EXT;
1194 break;
1195 case 3:
1196 priv->mode = UB953_MODE_NONSYNC_INT;
1197 break;
1198 case 5:
1199 priv->mode = UB953_MODE_DVP;
1200 break;
1201 default:
1202 return dev_err_probe(dev, -EIO,
1203 "Invalid mode in mode register\n");
1204 }
1205
1206 dev_dbg(dev, "mode from %s: %#x\n", mode_override ? "reg" : "strap",
1207 priv->mode);
1208
1209 if (priv->mode != UB953_MODE_SYNC &&
1210 priv->mode != UB953_MODE_NONSYNC_EXT)
1211 return dev_err_probe(dev, -ENODEV,
1212 "Unsupported mode selected: %u\n",
1213 priv->mode);
1214
1215 if (priv->mode == UB953_MODE_NONSYNC_EXT && !priv->clkin)
1216 return dev_err_probe(dev, -EINVAL,
1217 "clkin required for non-sync ext mode\n");
1218
1219 ret = ub953_read(priv, UB953_REG_REV_MASK_ID, &v, NULL);
1220 if (ret)
1221 return dev_err_probe(dev, ret, "Failed to read revision");
1222
1223 dev_info(dev, "Found %s rev/mask %#04x\n", priv->hw_data->model, v);
1224
1225 ret = ub953_read(priv, UB953_REG_GENERAL_CFG, &v, NULL);
1226 if (ret)
1227 return ret;
1228
1229 dev_dbg(dev, "i2c strap setting %s V\n",
1230 (v & UB953_REG_GENERAL_CFG_I2C_STRAP_MODE) ? "1.8" : "3.3");
1231
1232 ret = ub953_i2c_master_init(priv);
1233 if (ret)
1234 return dev_err_probe(dev, ret, "i2c init failed\n");
1235
1236 v = 0;
1237 v |= priv->non_continous_clk ? 0 : UB953_REG_GENERAL_CFG_CONT_CLK;
1238 v |= (priv->num_data_lanes - 1) <<
1239 UB953_REG_GENERAL_CFG_CSI_LANE_SEL_SHIFT;
1240 v |= UB953_REG_GENERAL_CFG_CRC_TX_GEN_ENABLE;
1241
1242 ret = ub953_write(priv, UB953_REG_GENERAL_CFG, v, NULL);
1243 if (ret)
1244 return ret;
1245
1246 v = 1U << UB953_REG_I2C_CONTROL2_SDA_OUTPUT_SETUP_SHIFT;
1247 v |= UB953_REG_I2C_CONTROL2_BUS_SPEEDUP;
1248
1249 ret = ub953_write(priv, UB953_REG_I2C_CONTROL2, v, NULL);
1250
1251 return ret;
1252 }
1253
ub953_subdev_init(struct ub953_data * priv)1254 static int ub953_subdev_init(struct ub953_data *priv)
1255 {
1256 struct device *dev = &priv->client->dev;
1257 int ret;
1258
1259 v4l2_i2c_subdev_init(&priv->sd, priv->client, &ub953_subdev_ops);
1260 priv->sd.internal_ops = &ub953_internal_ops;
1261
1262 priv->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
1263 V4L2_SUBDEV_FL_STREAMS;
1264 priv->sd.entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1265 priv->sd.entity.ops = &ub953_entity_ops;
1266
1267 priv->pads[0].flags = MEDIA_PAD_FL_SINK;
1268 priv->pads[1].flags = MEDIA_PAD_FL_SOURCE;
1269
1270 ret = media_entity_pads_init(&priv->sd.entity, 2, priv->pads);
1271 if (ret)
1272 return dev_err_probe(dev, ret, "Failed to init pads\n");
1273
1274 ret = v4l2_subdev_init_finalize(&priv->sd);
1275 if (ret)
1276 goto err_entity_cleanup;
1277
1278 ret = ub953_v4l2_notifier_register(priv);
1279 if (ret) {
1280 dev_err_probe(dev, ret,
1281 "v4l2 subdev notifier register failed\n");
1282 goto err_free_state;
1283 }
1284
1285 ret = v4l2_async_register_subdev(&priv->sd);
1286 if (ret) {
1287 dev_err_probe(dev, ret, "v4l2_async_register_subdev error\n");
1288 goto err_unreg_notif;
1289 }
1290
1291 return 0;
1292
1293 err_unreg_notif:
1294 ub953_v4l2_notifier_unregister(priv);
1295 err_free_state:
1296 v4l2_subdev_cleanup(&priv->sd);
1297 err_entity_cleanup:
1298 media_entity_cleanup(&priv->sd.entity);
1299
1300 return ret;
1301 }
1302
ub953_subdev_uninit(struct ub953_data * priv)1303 static void ub953_subdev_uninit(struct ub953_data *priv)
1304 {
1305 v4l2_async_unregister_subdev(&priv->sd);
1306 ub953_v4l2_notifier_unregister(priv);
1307 v4l2_subdev_cleanup(&priv->sd);
1308 media_entity_cleanup(&priv->sd.entity);
1309 }
1310
ub953_probe(struct i2c_client * client)1311 static int ub953_probe(struct i2c_client *client)
1312 {
1313 struct device *dev = &client->dev;
1314 struct ub953_data *priv;
1315 int ret;
1316
1317 priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
1318 if (!priv)
1319 return -ENOMEM;
1320
1321 priv->client = client;
1322
1323 priv->hw_data = device_get_match_data(dev);
1324
1325 priv->plat_data = dev_get_platdata(&client->dev);
1326 if (!priv->plat_data)
1327 return dev_err_probe(dev, -ENODEV, "Platform data missing\n");
1328
1329 mutex_init(&priv->reg_lock);
1330
1331 /*
1332 * Initialize to invalid values so that the first reg writes will
1333 * configure the target.
1334 */
1335 priv->current_indirect_target = 0xff;
1336
1337 priv->regmap = devm_regmap_init_i2c(client, &ub953_regmap_config);
1338 if (IS_ERR(priv->regmap)) {
1339 ret = PTR_ERR(priv->regmap);
1340 dev_err_probe(dev, ret, "Failed to init regmap\n");
1341 goto err_mutex_destroy;
1342 }
1343
1344 priv->clkin = devm_clk_get_optional(dev, "clkin");
1345 if (IS_ERR(priv->clkin)) {
1346 ret = PTR_ERR(priv->clkin);
1347 dev_err_probe(dev, ret, "failed to parse 'clkin'\n");
1348 goto err_mutex_destroy;
1349 }
1350
1351 ret = ub953_parse_dt(priv);
1352 if (ret)
1353 goto err_mutex_destroy;
1354
1355 ret = ub953_hw_init(priv);
1356 if (ret)
1357 goto err_mutex_destroy;
1358
1359 ret = ub953_gpiochip_probe(priv);
1360 if (ret) {
1361 dev_err_probe(dev, ret, "Failed to init gpiochip\n");
1362 goto err_mutex_destroy;
1363 }
1364
1365 ret = ub953_register_clkout(priv);
1366 if (ret) {
1367 dev_err_probe(dev, ret, "Failed to register clkout\n");
1368 goto err_gpiochip_remove;
1369 }
1370
1371 ret = ub953_subdev_init(priv);
1372 if (ret)
1373 goto err_gpiochip_remove;
1374
1375 ret = ub953_add_i2c_adapter(priv);
1376 if (ret) {
1377 dev_err_probe(dev, ret, "failed to add remote i2c adapter\n");
1378 goto err_subdev_uninit;
1379 }
1380
1381 return 0;
1382
1383 err_subdev_uninit:
1384 ub953_subdev_uninit(priv);
1385 err_gpiochip_remove:
1386 ub953_gpiochip_remove(priv);
1387 err_mutex_destroy:
1388 mutex_destroy(&priv->reg_lock);
1389
1390 return ret;
1391 }
1392
ub953_remove(struct i2c_client * client)1393 static void ub953_remove(struct i2c_client *client)
1394 {
1395 struct v4l2_subdev *sd = i2c_get_clientdata(client);
1396 struct ub953_data *priv = sd_to_ub953(sd);
1397
1398 i2c_atr_del_adapter(priv->plat_data->atr, priv->plat_data->port);
1399
1400 ub953_subdev_uninit(priv);
1401
1402 ub953_gpiochip_remove(priv);
1403 mutex_destroy(&priv->reg_lock);
1404 }
1405
1406 static const struct ub953_hw_data ds90ub953_hw = {
1407 .model = "ub953",
1408 };
1409
1410 static const struct ub953_hw_data ds90ub971_hw = {
1411 .model = "ub971",
1412 .is_ub971 = true,
1413 };
1414
1415 static const struct i2c_device_id ub953_id[] = {
1416 { "ds90ub953-q1", (kernel_ulong_t)&ds90ub953_hw },
1417 { "ds90ub971-q1", (kernel_ulong_t)&ds90ub971_hw },
1418 {}
1419 };
1420 MODULE_DEVICE_TABLE(i2c, ub953_id);
1421
1422 static const struct of_device_id ub953_dt_ids[] = {
1423 { .compatible = "ti,ds90ub953-q1", .data = &ds90ub953_hw },
1424 { .compatible = "ti,ds90ub971-q1", .data = &ds90ub971_hw },
1425 {}
1426 };
1427 MODULE_DEVICE_TABLE(of, ub953_dt_ids);
1428
1429 static struct i2c_driver ds90ub953_driver = {
1430 .probe = ub953_probe,
1431 .remove = ub953_remove,
1432 .id_table = ub953_id,
1433 .driver = {
1434 .name = "ds90ub953",
1435 .of_match_table = ub953_dt_ids,
1436 },
1437 };
1438 module_i2c_driver(ds90ub953_driver);
1439
1440 MODULE_LICENSE("GPL");
1441 MODULE_DESCRIPTION("Texas Instruments FPD-Link III/IV CSI-2 Serializers Driver");
1442 MODULE_AUTHOR("Luca Ceresoli <luca@lucaceresoli.net>");
1443 MODULE_AUTHOR("Tomi Valkeinen <tomi.valkeinen@ideasonboard.com>");
1444 MODULE_IMPORT_NS("I2C_ATR");
1445