ds90ub953.c

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