tc358746.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * TC358746 - Parallel <-> CSI-2 Bridge
   4 *
   5 * Copyright 2022 Marco Felsch <kernel@pengutronix.de>
   6 *
   7 * Notes:
   8 *  - Currently only 'Parallel-in -> CSI-out' mode is supported!
   9 */
  10
  11#include <linux/bitfield.h>
  12#include <linux/clk.h>
  13#include <linux/clk-provider.h>
  14#include <linux/delay.h>
  15#include <linux/i2c.h>
  16#include <linux/interrupt.h>
  17#include <linux/kernel.h>
  18#include <linux/module.h>
  19#include <linux/phy/phy-mipi-dphy.h>
  20#include <linux/property.h>
  21#include <linux/pm_runtime.h>
  22#include <linux/regmap.h>
  23#include <linux/units.h>
  24#include <media/v4l2-ctrls.h>
  25#include <media/v4l2-device.h>
  26#include <media/v4l2-fwnode.h>
  27#include <media/v4l2-mc.h>
  28
  29/* 16-bit registers */
  30#define CHIPID_REG			0x0000
  31#define		CHIPID			GENMASK(15, 8)
  32
  33#define SYSCTL_REG			0x0002
  34#define		SRESET			BIT(0)
  35
  36#define CONFCTL_REG			0x0004
  37#define		PDATAF_MASK		GENMASK(9, 8)
  38#define		PDATAF_MODE0		0
  39#define		PDATAF_MODE1		1
  40#define		PDATAF_MODE2		2
  41#define		PDATAF(val)		FIELD_PREP(PDATAF_MASK, (val))
  42#define		PPEN			BIT(6)
  43#define		DATALANE_MASK		GENMASK(1, 0)
  44
  45#define FIFOCTL_REG			0x0006
  46#define DATAFMT_REG			0x0008
  47#define		PDFMT(val)		FIELD_PREP(GENMASK(7, 4), (val))
  48
  49#define MCLKCTL_REG			0x000c
  50#define		MCLK_HIGH_MASK		GENMASK(15, 8)
  51#define		MCLK_LOW_MASK		GENMASK(7, 0)
  52#define		MCLK_HIGH(val)		FIELD_PREP(MCLK_HIGH_MASK, (val))
  53#define		MCLK_LOW(val)		FIELD_PREP(MCLK_LOW_MASK, (val))
  54
  55#define PLLCTL0_REG			0x0016
  56#define		PLL_PRD_MASK		GENMASK(15, 12)
  57#define		PLL_PRD(val)		FIELD_PREP(PLL_PRD_MASK, (val))
  58#define		PLL_FBD_MASK		GENMASK(8, 0)
  59#define		PLL_FBD(val)		FIELD_PREP(PLL_FBD_MASK, (val))
  60
  61#define PLLCTL1_REG			0x0018
  62#define		PLL_FRS_MASK		GENMASK(11, 10)
  63#define		PLL_FRS(val)		FIELD_PREP(PLL_FRS_MASK, (val))
  64#define		CKEN			BIT(4)
  65#define		RESETB			BIT(1)
  66#define		PLL_EN			BIT(0)
  67
  68#define CLKCTL_REG			0x0020
  69#define		MCLKDIV_MASK		GENMASK(3, 2)
  70#define		MCLKDIV(val)		FIELD_PREP(MCLKDIV_MASK, (val))
  71#define		MCLKDIV_8		0
  72#define		MCLKDIV_4		1
  73#define		MCLKDIV_2		2
  74
  75#define WORDCNT_REG			0x0022
  76#define PP_MISC_REG			0x0032
  77#define		FRMSTOP			BIT(15)
  78#define		RSTPTR			BIT(14)
  79
  80/* 32-bit registers */
  81#define CLW_DPHYCONTTX_REG		0x0100
  82#define CLW_CNTRL_REG			0x0140
  83#define D0W_CNTRL_REG			0x0144
  84#define		LANEDISABLE		BIT(0)
  85
  86#define STARTCNTRL_REG			0x0204
  87#define		START			BIT(0)
  88
  89#define LINEINITCNT_REG			0x0210
  90#define LPTXTIMECNT_REG			0x0214
  91#define TCLK_HEADERCNT_REG		0x0218
  92#define		TCLK_ZEROCNT(val)	FIELD_PREP(GENMASK(15, 8), (val))
  93#define		TCLK_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
  94
  95#define TCLK_TRAILCNT_REG		0x021C
  96#define THS_HEADERCNT_REG		0x0220
  97#define		THS_ZEROCNT(val)	FIELD_PREP(GENMASK(14, 8), (val))
  98#define		THS_PREPARECNT(val)	FIELD_PREP(GENMASK(6, 0), (val))
  99
 100#define TWAKEUP_REG			0x0224
 101#define TCLK_POSTCNT_REG		0x0228
 102#define THS_TRAILCNT_REG		0x022C
 103#define HSTXVREGEN_REG			0x0234
 104#define TXOPTIONCNTRL_REG		0x0238
 105#define CSI_CONTROL_REG			0x040C
 106#define		CSI_MODE		BIT(15)
 107#define		TXHSMD			BIT(7)
 108#define		NOL(val)		FIELD_PREP(GENMASK(2, 1), (val))
 109
 110#define CSI_CONFW_REG			0x0500
 111#define		MODE(val)		FIELD_PREP(GENMASK(31, 29), (val))
 112#define		MODE_SET		0x5
 113#define		ADDRESS(val)		FIELD_PREP(GENMASK(28, 24), (val))
 114#define		CSI_CONTROL_ADDRESS	0x3
 115#define		DATA(val)		FIELD_PREP(GENMASK(15, 0), (val))
 116
 117#define CSI_START_REG			0x0518
 118#define		STRT			BIT(0)
 119
 120static const struct v4l2_mbus_framefmt tc358746_def_fmt = {
 121	.width		= 640,
 122	.height		= 480,
 123	.code		= MEDIA_BUS_FMT_UYVY8_2X8,
 124	.field		= V4L2_FIELD_NONE,
 125	.colorspace	= V4L2_COLORSPACE_DEFAULT,
 126	.ycbcr_enc	= V4L2_YCBCR_ENC_DEFAULT,
 127	.quantization	= V4L2_QUANTIZATION_DEFAULT,
 128	.xfer_func	= V4L2_XFER_FUNC_DEFAULT,
 129};
 130
 131static const char * const tc358746_supplies[] = {
 132	"vddc", "vddio", "vddmipi"
 133};
 134
 135enum {
 136	TC358746_SINK,
 137	TC358746_SOURCE,
 138	TC358746_NR_PADS
 139};
 140
 141struct tc358746 {
 142	struct v4l2_subdev		sd;
 143	struct media_pad		pads[TC358746_NR_PADS];
 144	struct v4l2_async_notifier	notifier;
 145	struct v4l2_fwnode_endpoint	csi_vep;
 146
 147	struct v4l2_ctrl_handler	ctrl_hdl;
 148
 149	struct regmap			*regmap;
 150	struct clk			*refclk;
 151	struct gpio_desc		*reset_gpio;
 152	struct regulator_bulk_data	supplies[ARRAY_SIZE(tc358746_supplies)];
 153
 154	struct clk_hw			mclk_hw;
 155	unsigned long			mclk_rate;
 156	u8				mclk_prediv;
 157	u16				mclk_postdiv;
 158
 159	unsigned long			pll_rate;
 160	u8				pll_post_div;
 161	u16				pll_pre_div;
 162	u16				pll_mul;
 163
 164	struct phy_configure_opts_mipi_dphy dphy_cfg;
 165};
 166
 167static inline struct tc358746 *to_tc358746(struct v4l2_subdev *sd)
 168{
 169	return container_of(sd, struct tc358746, sd);
 170}
 171
 172static inline struct tc358746 *clk_hw_to_tc358746(struct clk_hw *hw)
 173{
 174	return container_of(hw, struct tc358746, mclk_hw);
 175}
 176
 177struct tc358746_format {
 178	u32		code;
 179	bool		csi_format;
 180	unsigned char	bus_width;
 181	unsigned char	bpp;
 182	/* Register values */
 183	u8		pdformat; /* Peripheral Data Format */
 184	u8		pdataf;   /* Parallel Data Format Option */
 185};
 186
 187enum {
 188	PDFORMAT_RAW8 = 0,
 189	PDFORMAT_RAW10,
 190	PDFORMAT_RAW12,
 191	PDFORMAT_RGB888,
 192	PDFORMAT_RGB666,
 193	PDFORMAT_RGB565,
 194	PDFORMAT_YUV422_8BIT,
 195	/* RESERVED = 7 */
 196	PDFORMAT_RAW14 = 8,
 197	PDFORMAT_YUV422_10BIT,
 198	PDFORMAT_YUV444,
 199};
 200
 201#define TC358746_FORMAT_RAW(_bpp, _code)		\
 202{							\
 203	.code = _code,					\
 204	.bus_width = _bpp,				\
 205	.bpp = _bpp,					\
 206	.pdformat = PDFORMAT_RAW##_bpp,			\
 207	.pdataf = PDATAF_MODE0, /* don't care */	\
 208}
 209
 210/* Check tc358746_src_mbus_code() if you add new formats */
 211static const struct tc358746_format tc358746_formats[] = {
 212	{
 213		.code = MEDIA_BUS_FMT_UYVY8_2X8,
 214		.bus_width = 8,
 215		.bpp = 16,
 216		.pdformat = PDFORMAT_YUV422_8BIT,
 217		.pdataf = PDATAF_MODE0,
 218	}, {
 219		.code = MEDIA_BUS_FMT_UYVY8_1X16,
 220		.csi_format = true,
 221		.bus_width = 16,
 222		.bpp = 16,
 223		.pdformat = PDFORMAT_YUV422_8BIT,
 224		.pdataf = PDATAF_MODE1,
 225	}, {
 226		.code = MEDIA_BUS_FMT_YUYV8_1X16,
 227		.csi_format = true,
 228		.bus_width = 16,
 229		.bpp = 16,
 230		.pdformat = PDFORMAT_YUV422_8BIT,
 231		.pdataf = PDATAF_MODE2,
 232	}, {
 233		.code = MEDIA_BUS_FMT_UYVY10_2X10,
 234		.bus_width = 10,
 235		.bpp = 20,
 236		.pdformat = PDFORMAT_YUV422_10BIT,
 237		.pdataf = PDATAF_MODE0, /* don't care */
 238	},
 239	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SBGGR8_1X8),
 240	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SGBRG8_1X8),
 241	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SGRBG8_1X8),
 242	TC358746_FORMAT_RAW(8, MEDIA_BUS_FMT_SRGGB8_1X8),
 243	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SBGGR10_1X10),
 244	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SGBRG10_1X10),
 245	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SGRBG10_1X10),
 246	TC358746_FORMAT_RAW(10, MEDIA_BUS_FMT_SRGGB10_1X10),
 247	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SBGGR12_1X12),
 248	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SGBRG12_1X12),
 249	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SGRBG12_1X12),
 250	TC358746_FORMAT_RAW(12, MEDIA_BUS_FMT_SRGGB12_1X12),
 251	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SBGGR14_1X14),
 252	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SGBRG14_1X14),
 253	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SGRBG14_1X14),
 254	TC358746_FORMAT_RAW(14, MEDIA_BUS_FMT_SRGGB14_1X14),
 255};
 256
 257/* Get n-th format for pad */
 258static const struct tc358746_format *
 259tc358746_get_format_by_idx(unsigned int pad, unsigned int index)
 260{
 261	unsigned int idx = 0;
 262	unsigned int i;
 263
 264	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
 265		const struct tc358746_format *fmt = &tc358746_formats[i];
 266
 267		if ((pad == TC358746_SOURCE && fmt->csi_format) ||
 268		    (pad == TC358746_SINK)) {
 269			if (idx == index)
 270				return fmt;
 271			idx++;
 272		}
 273	}
 274
 275	return ERR_PTR(-EINVAL);
 276}
 277
 278static const struct tc358746_format *
 279tc358746_get_format_by_code(unsigned int pad, u32 code)
 280{
 281	unsigned int i;
 282
 283	for (i = 0; i < ARRAY_SIZE(tc358746_formats); i++) {
 284		const struct tc358746_format *fmt = &tc358746_formats[i];
 285
 286		if (pad == TC358746_SINK && fmt->code == code)
 287			return fmt;
 288
 289		if (pad == TC358746_SOURCE && !fmt->csi_format)
 290			continue;
 291
 292		if (fmt->code == code)
 293			return fmt;
 294	}
 295
 296	return ERR_PTR(-EINVAL);
 297}
 298
 299static u32 tc358746_src_mbus_code(u32 code)
 300{
 301	switch (code) {
 302	case MEDIA_BUS_FMT_UYVY8_2X8:
 303		return MEDIA_BUS_FMT_UYVY8_1X16;
 304	case MEDIA_BUS_FMT_UYVY10_2X10:
 305		return MEDIA_BUS_FMT_UYVY10_1X20;
 306	default:
 307		return code;
 308	}
 309}
 310
 311static bool tc358746_valid_reg(struct device *dev, unsigned int reg)
 312{
 313	switch (reg) {
 314	case CHIPID_REG ... CSI_START_REG:
 315		return true;
 316	default:
 317		return false;
 318	}
 319}
 320
 321static const struct regmap_config tc358746_regmap_config = {
 322	.name = "tc358746",
 323	.reg_bits = 16,
 324	.val_bits = 16,
 325	.max_register = CSI_START_REG,
 326	.writeable_reg = tc358746_valid_reg,
 327	.readable_reg = tc358746_valid_reg,
 328	.reg_format_endian = REGMAP_ENDIAN_BIG,
 329	.val_format_endian = REGMAP_ENDIAN_BIG,
 330};
 331
 332static int tc358746_write(struct tc358746 *tc358746, u32 reg, u32 val)
 333{
 334	size_t count;
 335	int err;
 336
 337	/* 32-bit registers starting from CLW_DPHYCONTTX */
 338	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
 339
 340	err = regmap_bulk_write(tc358746->regmap, reg, &val, count);
 341	if (err)
 342		dev_err(tc358746->sd.dev,
 343			"Failed to write reg:0x%04x err:%d\n", reg, err);
 344
 345	return err;
 346}
 347
 348static int tc358746_read(struct tc358746 *tc358746, u32 reg, u32 *val)
 349{
 350	size_t count;
 351	int err;
 352
 353	/* 32-bit registers starting from CLW_DPHYCONTTX */
 354	count = reg < CLW_DPHYCONTTX_REG ? 1 : 2;
 355	*val = 0;
 356
 357	err = regmap_bulk_read(tc358746->regmap, reg, val, count);
 358	if (err)
 359		dev_err(tc358746->sd.dev,
 360			"Failed to read reg:0x%04x err:%d\n", reg, err);
 361
 362	return err;
 363}
 364
 365static int
 366tc358746_update_bits(struct tc358746 *tc358746, u32 reg, u32 mask, u32 val)
 367{
 368	u32 tmp, orig;
 369	int err;
 370
 371	err = tc358746_read(tc358746, reg, &orig);
 372	if (err)
 373		return err;
 374
 375	tmp = orig & ~mask;
 376	tmp |= val & mask;
 377
 378	return tc358746_write(tc358746, reg, tmp);
 379}
 380
 381static int tc358746_set_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
 382{
 383	return tc358746_update_bits(tc358746, reg, bits, bits);
 384}
 385
 386static int tc358746_clear_bits(struct tc358746 *tc358746, u32 reg, u32 bits)
 387{
 388	return tc358746_update_bits(tc358746, reg, bits, 0);
 389}
 390
 391static int tc358746_sw_reset(struct tc358746 *tc358746)
 392{
 393	int err;
 394
 395	err = tc358746_set_bits(tc358746, SYSCTL_REG, SRESET);
 396	if (err)
 397		return err;
 398
 399	fsleep(10);
 400
 401	return tc358746_clear_bits(tc358746, SYSCTL_REG, SRESET);
 402}
 403
 404static int
 405tc358746_apply_pll_config(struct tc358746 *tc358746)
 406{
 407	u8 post = tc358746->pll_post_div;
 408	u16 pre = tc358746->pll_pre_div;
 409	u16 mul = tc358746->pll_mul;
 410	u32 val, mask;
 411	int err;
 412
 413	err = tc358746_read(tc358746, PLLCTL1_REG, &val);
 414	if (err)
 415		return err;
 416
 417	/* Don't touch the PLL if running */
 418	if (FIELD_GET(PLL_EN, val) == 1)
 419		return 0;
 420
 421	/* Pre-div and Multiplicator have a internal +1 logic */
 422	val = PLL_PRD(pre - 1) | PLL_FBD(mul - 1);
 423	mask = PLL_PRD_MASK | PLL_FBD_MASK;
 424	err = tc358746_update_bits(tc358746, PLLCTL0_REG, mask, val);
 425	if (err)
 426		return err;
 427
 428	val = PLL_FRS(ilog2(post)) | RESETB | PLL_EN;
 429	mask = PLL_FRS_MASK | RESETB | PLL_EN;
 430	err = tc358746_update_bits(tc358746, PLLCTL1_REG, mask, val);
 431	if (err)
 432		return err;
 433
 434	fsleep(1000);
 435
 436	return tc358746_set_bits(tc358746, PLLCTL1_REG, CKEN);
 437}
 438
 439#define TC358746_VB_PRECISION		10
 440#define TC358746_VB_MAX_SIZE		(511 * 32)
 441#define TC358746_VB_DEFAULT_SIZE	(1 * 32)
 442
 443static int tc358746_calc_vb_size(struct tc358746 *tc358746,
 444				 s64 source_link_freq,
 445				 const struct v4l2_mbus_framefmt *mbusfmt,
 446				 const struct tc358746_format *fmt)
 447{
 448	unsigned long csi_bitrate, source_bitrate;
 449	unsigned int fifo_sz, tmp, n;
 450	int vb_size; /* Video buffer size in bits */
 451
 452	source_bitrate = source_link_freq * fmt->bus_width;
 453
 454	csi_bitrate = tc358746->dphy_cfg.lanes * tc358746->pll_rate;
 455
 456	dev_dbg(tc358746->sd.dev,
 457		"Fifo settings params: source-bitrate:%lu csi-bitrate:%lu",
 458		source_bitrate, csi_bitrate);
 459
 460	/* Avoid possible FIFO overflows */
 461	if (csi_bitrate < source_bitrate)
 462		return -EINVAL;
 463
 464	/* Best case */
 465	if (csi_bitrate == source_bitrate) {
 466		fifo_sz = TC358746_VB_DEFAULT_SIZE;
 467		vb_size = TC358746_VB_DEFAULT_SIZE;
 468	} else {
 469		/*
 470		 * Avoid possible FIFO underflow in case of
 471		 * csi_bitrate > source_bitrate. For such case the chip has a internal
 472		 * fifo which can be used to delay the line output.
 473		 *
 474		 * Fifo size calculation (excluding precision):
 475		 *
 476		 * fifo-sz, image-width - in bits
 477		 * sbr                  - source_bitrate in bits/s
 478		 * csir                 - csi_bitrate in bits/s
 479		 *
 480		 * image-width / csir >= (image-width - fifo-sz) / sbr
 481		 * image-width * sbr / csir >= image-width - fifo-sz
 482		 * fifo-sz >= image-width - image-width * sbr / csir; with n = csir/sbr
 483		 * fifo-sz >= image-width - image-width / n
 484		 */
 485		source_bitrate /= TC358746_VB_PRECISION;
 486		n = csi_bitrate / source_bitrate;
 487		tmp = (mbusfmt->width * TC358746_VB_PRECISION) / n;
 488		fifo_sz = mbusfmt->width - tmp;
 489		fifo_sz *= fmt->bpp;
 490		vb_size = round_up(fifo_sz, 32);
 491	}
 492
 493	dev_dbg(tc358746->sd.dev,
 494		"Found FIFO size[bits]:%u -> aligned to size[bits]:%u\n",
 495		fifo_sz, vb_size);
 496
 497	if (vb_size > TC358746_VB_MAX_SIZE)
 498		return -EINVAL;
 499
 500	return vb_size;
 501}
 502
 503static int tc358746_apply_misc_config(struct tc358746 *tc358746)
 504{
 505	const struct v4l2_mbus_framefmt *mbusfmt;
 506	struct v4l2_subdev *sd = &tc358746->sd;
 507	struct v4l2_subdev_state *sink_state;
 508	const struct tc358746_format *fmt;
 509	struct device *dev = sd->dev;
 510	struct media_pad *source_pad;
 511	s64 source_link_freq;
 512	int vb_size;
 513	u32 val;
 514	int err;
 515
 516	sink_state = v4l2_subdev_lock_and_get_active_state(sd);
 517
 518	mbusfmt = v4l2_subdev_state_get_format(sink_state, TC358746_SINK);
 519	fmt = tc358746_get_format_by_code(TC358746_SINK, mbusfmt->code);
 520
 521	source_pad = media_entity_remote_source_pad_unique(&sd->entity);
 522	if (IS_ERR(source_pad)) {
 523		dev_err(dev, "Failed to get source pad of %s\n", sd->name);
 524		err = PTR_ERR(source_pad);
 525		goto out;
 526	}
 527	source_link_freq = v4l2_get_link_freq(source_pad, 0, 0);
 528	if (source_link_freq <= 0) {
 529		dev_err(dev,
 530			"Failed to query or invalid source link frequency\n");
 531		/* Return -EINVAL in case of source_link_freq is 0 */
 532		err = source_link_freq ?: -EINVAL;
 533		goto out;
 534	}
 535
 536	/* Self defined CSI user data type id's are not supported yet */
 537	val = PDFMT(fmt->pdformat);
 538	dev_dbg(dev, "DATAFMT: 0x%x\n", val);
 539	err = tc358746_write(tc358746, DATAFMT_REG, val);
 540	if (err)
 541		goto out;
 542
 543	val = PDATAF(fmt->pdataf);
 544	dev_dbg(dev, "CONFCTL[PDATAF]: 0x%x\n", fmt->pdataf);
 545	err = tc358746_update_bits(tc358746, CONFCTL_REG, PDATAF_MASK, val);
 546	if (err)
 547		goto out;
 548
 549	vb_size = tc358746_calc_vb_size(tc358746, source_link_freq, mbusfmt, fmt);
 550	if (vb_size < 0) {
 551		err = vb_size;
 552		goto out;
 553	}
 554
 555	val = vb_size / 32;
 556	dev_dbg(dev, "FIFOCTL: %u (0x%x)\n", val, val);
 557	err = tc358746_write(tc358746, FIFOCTL_REG, val);
 558	if (err)
 559		goto out;
 560
 561	/* Total number of bytes for each line/width */
 562	val = mbusfmt->width * fmt->bpp / 8;
 563	dev_dbg(dev, "WORDCNT: %u (0x%x)\n", val, val);
 564	err = tc358746_write(tc358746, WORDCNT_REG, val);
 565
 566out:
 567	v4l2_subdev_unlock_state(sink_state);
 568
 569	return err;
 570}
 571
 572static u32 tc358746_cfg_to_cnt(unsigned long cfg_val, unsigned long clk_hz,
 573			       unsigned long long time_base)
 574{
 575	return div64_u64((u64)cfg_val * clk_hz + time_base - 1, time_base);
 576}
 577
 578static u32 tc358746_ps_to_cnt(unsigned long cfg_val, unsigned long clk_hz)
 579{
 580	return tc358746_cfg_to_cnt(cfg_val, clk_hz, PSEC_PER_SEC);
 581}
 582
 583static u32 tc358746_us_to_cnt(unsigned long cfg_val, unsigned long clk_hz)
 584{
 585	return tc358746_cfg_to_cnt(cfg_val, clk_hz, USEC_PER_SEC);
 586}
 587
 588static int tc358746_apply_dphy_config(struct tc358746 *tc358746)
 589{
 590	struct phy_configure_opts_mipi_dphy *cfg = &tc358746->dphy_cfg;
 591	bool non_cont_clk = !!(tc358746->csi_vep.bus.mipi_csi2.flags &
 592			       V4L2_MBUS_CSI2_NONCONTINUOUS_CLOCK);
 593	struct device *dev = tc358746->sd.dev;
 594	unsigned long hs_byte_clk, hf_clk;
 595	u32 val, val2, lptxcnt;
 596	int err;
 597
 598	/* The hs_byte_clk is also called SYSCLK in the excel sheet */
 599	hs_byte_clk = cfg->hs_clk_rate / 8;
 600	hf_clk = hs_byte_clk / 2;
 601
 602	val = tc358746_us_to_cnt(cfg->init, hf_clk) - 1;
 603	dev_dbg(dev, "LINEINITCNT: %u (0x%x)\n", val, val);
 604	err = tc358746_write(tc358746, LINEINITCNT_REG, val);
 605	if (err)
 606		return err;
 607
 608	val = tc358746_ps_to_cnt(cfg->lpx, hs_byte_clk) - 1;
 609	lptxcnt = val;
 610	dev_dbg(dev, "LPTXTIMECNT: %u (0x%x)\n", val, val);
 611	err = tc358746_write(tc358746, LPTXTIMECNT_REG, val);
 612	if (err)
 613		return err;
 614
 615	val = tc358746_ps_to_cnt(cfg->clk_prepare, hs_byte_clk) - 1;
 616	val2 = tc358746_ps_to_cnt(cfg->clk_zero, hs_byte_clk) - 1;
 617	dev_dbg(dev, "TCLK_PREPARECNT: %u (0x%x)\n", val, val);
 618	dev_dbg(dev, "TCLK_ZEROCNT: %u (0x%x)\n", val2, val2);
 619	dev_dbg(dev, "TCLK_HEADERCNT: 0x%x\n",
 620		(u32)(TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2)));
 621	err = tc358746_write(tc358746, TCLK_HEADERCNT_REG,
 622			     TCLK_PREPARECNT(val) | TCLK_ZEROCNT(val2));
 623	if (err)
 624		return err;
 625
 626	val = tc358746_ps_to_cnt(cfg->clk_trail, hs_byte_clk);
 627	dev_dbg(dev, "TCLK_TRAILCNT: %u (0x%x)\n", val, val);
 628	err = tc358746_write(tc358746, TCLK_TRAILCNT_REG, val);
 629	if (err)
 630		return err;
 631
 632	val = tc358746_ps_to_cnt(cfg->hs_prepare, hs_byte_clk) - 1;
 633	val2 = tc358746_ps_to_cnt(cfg->hs_zero, hs_byte_clk) - 1;
 634	dev_dbg(dev, "THS_PREPARECNT: %u (0x%x)\n", val, val);
 635	dev_dbg(dev, "THS_ZEROCNT: %u (0x%x)\n", val2, val2);
 636	dev_dbg(dev, "THS_HEADERCNT: 0x%x\n",
 637		(u32)(THS_PREPARECNT(val) | THS_ZEROCNT(val2)));
 638	err = tc358746_write(tc358746, THS_HEADERCNT_REG,
 639			     THS_PREPARECNT(val) | THS_ZEROCNT(val2));
 640	if (err)
 641		return err;
 642
 643	/* TWAKEUP > 1ms in lptxcnt steps */
 644	val = tc358746_us_to_cnt(cfg->wakeup, hs_byte_clk);
 645	val = val / (lptxcnt + 1) - 1;
 646	dev_dbg(dev, "TWAKEUP: %u (0x%x)\n", val, val);
 647	err = tc358746_write(tc358746, TWAKEUP_REG, val);
 648	if (err)
 649		return err;
 650
 651	val = tc358746_ps_to_cnt(cfg->clk_post, hs_byte_clk);
 652	dev_dbg(dev, "TCLK_POSTCNT: %u (0x%x)\n", val, val);
 653	err = tc358746_write(tc358746, TCLK_POSTCNT_REG, val);
 654	if (err)
 655		return err;
 656
 657	val = tc358746_ps_to_cnt(cfg->hs_trail, hs_byte_clk);
 658	dev_dbg(dev, "THS_TRAILCNT: %u (0x%x)\n", val, val);
 659	err = tc358746_write(tc358746, THS_TRAILCNT_REG, val);
 660	if (err)
 661		return err;
 662
 663	dev_dbg(dev, "CONTCLKMODE: %u", non_cont_clk ? 0 : 1);
 664
 665	return  tc358746_write(tc358746, TXOPTIONCNTRL_REG, non_cont_clk ? 0 : 1);
 666}
 667
 668#define MAX_DATA_LANES 4
 669
 670static int tc358746_enable_csi_lanes(struct tc358746 *tc358746, int enable)
 671{
 672	unsigned int lanes = tc358746->dphy_cfg.lanes;
 673	unsigned int lane;
 674	u32 reg, val;
 675	int err;
 676
 677	err = tc358746_update_bits(tc358746, CONFCTL_REG, DATALANE_MASK,
 678				   lanes - 1);
 679	if (err)
 680		return err;
 681
 682	/* Clock lane */
 683	val = enable ? 0 : LANEDISABLE;
 684	dev_dbg(tc358746->sd.dev, "CLW_CNTRL: 0x%x\n", val);
 685	err = tc358746_write(tc358746, CLW_CNTRL_REG, val);
 686	if (err)
 687		return err;
 688
 689	for (lane = 0; lane < MAX_DATA_LANES; lane++) {
 690		/* Data lanes */
 691		reg = D0W_CNTRL_REG + lane * 0x4;
 692		val = (enable && lane < lanes) ? 0 : LANEDISABLE;
 693
 694		dev_dbg(tc358746->sd.dev, "D%uW_CNTRL: 0x%x\n", lane, val);
 695		err = tc358746_write(tc358746, reg, val);
 696		if (err)
 697			return err;
 698	}
 699
 700	val = 0;
 701	if (enable) {
 702		/* Clock lane */
 703		val |= BIT(0);
 704
 705		/* Data lanes */
 706		for (lane = 1; lane <= lanes; lane++)
 707			val |= BIT(lane);
 708	}
 709
 710	dev_dbg(tc358746->sd.dev, "HSTXVREGEN: 0x%x\n", val);
 711
 712	return tc358746_write(tc358746, HSTXVREGEN_REG, val);
 713}
 714
 715static int tc358746_enable_csi_module(struct tc358746 *tc358746, int enable)
 716{
 717	unsigned int lanes = tc358746->dphy_cfg.lanes;
 718	int err;
 719
 720	/*
 721	 * START and STRT are only reseted/disabled by sw reset. This is
 722	 * required to put the lane state back into LP-11 state. The sw reset
 723	 * don't reset register values.
 724	 */
 725	if (!enable)
 726		return tc358746_sw_reset(tc358746);
 727
 728	err = tc358746_write(tc358746, STARTCNTRL_REG, START);
 729	if (err)
 730		return err;
 731
 732	err = tc358746_write(tc358746, CSI_START_REG, STRT);
 733	if (err)
 734		return err;
 735
 736	/* CSI_CONTROL_REG is only indirect accessible */
 737	return tc358746_write(tc358746, CSI_CONFW_REG,
 738			      MODE(MODE_SET) |
 739			      ADDRESS(CSI_CONTROL_ADDRESS) |
 740			      DATA(CSI_MODE | TXHSMD | NOL(lanes - 1)));
 741}
 742
 743static int tc358746_enable_parallel_port(struct tc358746 *tc358746, int enable)
 744{
 745	int err;
 746
 747	if (enable) {
 748		err = tc358746_write(tc358746, PP_MISC_REG, 0);
 749		if (err)
 750			return err;
 751
 752		return tc358746_set_bits(tc358746, CONFCTL_REG, PPEN);
 753	}
 754
 755	err = tc358746_set_bits(tc358746, PP_MISC_REG, FRMSTOP);
 756	if (err)
 757		return err;
 758
 759	err = tc358746_clear_bits(tc358746, CONFCTL_REG, PPEN);
 760	if (err)
 761		return err;
 762
 763	return tc358746_set_bits(tc358746, PP_MISC_REG, RSTPTR);
 764}
 765
 766static inline struct v4l2_subdev *tc358746_get_remote_sd(struct media_pad *pad)
 767{
 768	pad = media_pad_remote_pad_first(pad);
 769	if (!pad)
 770		return NULL;
 771
 772	return media_entity_to_v4l2_subdev(pad->entity);
 773}
 774
 775static int tc358746_s_stream(struct v4l2_subdev *sd, int enable)
 776{
 777	struct tc358746 *tc358746 = to_tc358746(sd);
 778	struct v4l2_subdev *src;
 779	int err;
 780
 781	dev_dbg(sd->dev, "%sable\n", enable ? "en" : "dis");
 782
 783	src = tc358746_get_remote_sd(&tc358746->pads[TC358746_SINK]);
 784	if (!src)
 785		return -EPIPE;
 786
 787	if (enable) {
 788		err = pm_runtime_resume_and_get(sd->dev);
 789		if (err)
 790			return err;
 791
 792		err = tc358746_apply_dphy_config(tc358746);
 793		if (err)
 794			goto err_out;
 795
 796		err = tc358746_apply_misc_config(tc358746);
 797		if (err)
 798			goto err_out;
 799
 800		err = tc358746_enable_csi_lanes(tc358746, 1);
 801		if (err)
 802			goto err_out;
 803
 804		err = tc358746_enable_csi_module(tc358746, 1);
 805		if (err)
 806			goto err_out;
 807
 808		err = tc358746_enable_parallel_port(tc358746, 1);
 809		if (err)
 810			goto err_out;
 811
 812		err = v4l2_subdev_call(src, video, s_stream, 1);
 813		if (err)
 814			goto err_out;
 815
 816		return 0;
 817
 818err_out:
 819		pm_runtime_put_sync_autosuspend(sd->dev);
 820
 821		return err;
 822	}
 823
 824	/*
 825	 * The lanes must be disabled first (before the csi module) so the
 826	 * LP-11 state is entered correctly.
 827	 */
 828	err = tc358746_enable_csi_lanes(tc358746, 0);
 829	if (err)
 830		return err;
 831
 832	err = tc358746_enable_csi_module(tc358746, 0);
 833	if (err)
 834		return err;
 835
 836	err = tc358746_enable_parallel_port(tc358746, 0);
 837	if (err)
 838		return err;
 839
 840	pm_runtime_put_sync_autosuspend(sd->dev);
 841
 842	return v4l2_subdev_call(src, video, s_stream, 0);
 843}
 844
 845static int tc358746_init_state(struct v4l2_subdev *sd,
 846			       struct v4l2_subdev_state *state)
 847{
 848	struct v4l2_mbus_framefmt *fmt;
 849
 850	fmt = v4l2_subdev_state_get_format(state, TC358746_SINK);
 851	*fmt = tc358746_def_fmt;
 852
 853	fmt = v4l2_subdev_state_get_format(state, TC358746_SOURCE);
 854	*fmt = tc358746_def_fmt;
 855	fmt->code = tc358746_src_mbus_code(tc358746_def_fmt.code);
 856
 857	return 0;
 858}
 859
 860static int tc358746_enum_mbus_code(struct v4l2_subdev *sd,
 861				   struct v4l2_subdev_state *sd_state,
 862				   struct v4l2_subdev_mbus_code_enum *code)
 863{
 864	const struct tc358746_format *fmt;
 865
 866	fmt = tc358746_get_format_by_idx(code->pad, code->index);
 867	if (IS_ERR(fmt))
 868		return PTR_ERR(fmt);
 869
 870	code->code = fmt->code;
 871
 872	return 0;
 873}
 874
 875static int tc358746_set_fmt(struct v4l2_subdev *sd,
 876			    struct v4l2_subdev_state *sd_state,
 877			    struct v4l2_subdev_format *format)
 878{
 879	struct v4l2_mbus_framefmt *src_fmt, *sink_fmt;
 880	const struct tc358746_format *fmt;
 881
 882	/* Source follows the sink */
 883	if (format->pad == TC358746_SOURCE)
 884		return v4l2_subdev_get_fmt(sd, sd_state, format);
 885
 886	sink_fmt = v4l2_subdev_state_get_format(sd_state, TC358746_SINK);
 887
 888	fmt = tc358746_get_format_by_code(format->pad, format->format.code);
 889	if (IS_ERR(fmt)) {
 890		fmt = tc358746_get_format_by_code(format->pad, tc358746_def_fmt.code);
 891		// Can't happen, but just in case...
 892		if (WARN_ON(IS_ERR(fmt)))
 893			return -EINVAL;
 894	}
 895
 896	format->format.code = fmt->code;
 897	format->format.field = V4L2_FIELD_NONE;
 898
 899	dev_dbg(sd->dev, "Update format: %ux%u code:0x%x -> %ux%u code:0x%x",
 900		sink_fmt->width, sink_fmt->height, sink_fmt->code,
 901		format->format.width, format->format.height, format->format.code);
 902
 903	*sink_fmt = format->format;
 904
 905	src_fmt = v4l2_subdev_state_get_format(sd_state, TC358746_SOURCE);
 906	*src_fmt = *sink_fmt;
 907	src_fmt->code = tc358746_src_mbus_code(sink_fmt->code);
 908
 909	return 0;
 910}
 911
 912static unsigned long tc358746_find_pll_settings(struct tc358746 *tc358746,
 913						unsigned long refclk,
 914						unsigned long fout)
 915
 916{
 917	struct device *dev = tc358746->sd.dev;
 918	unsigned long best_freq = 0;
 919	u32 min_delta = 0xffffffff;
 920	u16 prediv_max = 17;
 921	u16 prediv_min = 1;
 922	u16 m_best = 0, mul;
 923	u16 p_best = 1, p;
 924	u8 postdiv;
 925
 926	if (fout > 1000 * HZ_PER_MHZ) {
 927		dev_err(dev, "HS-Clock above 1 Ghz are not supported\n");
 928		return 0;
 929	}
 930
 931	if (fout >= 500 * HZ_PER_MHZ)
 932		postdiv = 1;
 933	else if (fout >= 250 * HZ_PER_MHZ)
 934		postdiv = 2;
 935	else if (fout >= 125 * HZ_PER_MHZ)
 936		postdiv = 4;
 937	else
 938		postdiv = 8;
 939
 940	for (p = prediv_min; p <= prediv_max; p++) {
 941		unsigned long delta, fin;
 942		u64 tmp;
 943
 944		fin = DIV_ROUND_CLOSEST(refclk, p);
 945		if (fin < 4 * HZ_PER_MHZ || fin > 40 * HZ_PER_MHZ)
 946			continue;
 947
 948		tmp = fout * postdiv;
 949		mul = div64_ul(tmp, fin);
 950		if (mul > 511)
 951			continue;
 952
 953		tmp = mul * fin;
 954		do_div(tmp, postdiv);
 955
 956		delta = abs(fout - tmp);
 957		if (delta < min_delta) {
 958			p_best = p;
 959			m_best = mul;
 960			min_delta = delta;
 961			best_freq = tmp;
 962		}
 963
 964		if (delta == 0)
 965			break;
 966	}
 967
 968	if (!best_freq) {
 969		dev_err(dev, "Failed find PLL frequency\n");
 970		return 0;
 971	}
 972
 973	tc358746->pll_post_div = postdiv;
 974	tc358746->pll_pre_div = p_best;
 975	tc358746->pll_mul = m_best;
 976
 977	if (best_freq != fout)
 978		dev_warn(dev, "Request PLL freq:%lu, found PLL freq:%lu\n",
 979			 fout, best_freq);
 980
 981	dev_dbg(dev, "Found PLL settings: freq:%lu prediv:%u multi:%u postdiv:%u\n",
 982		best_freq, p_best, m_best, postdiv);
 983
 984	return best_freq;
 985}
 986
 987static int tc358746_get_mbus_config(struct v4l2_subdev *sd, unsigned int pad,
 988				    struct v4l2_mbus_config *config)
 989{
 990	struct tc358746 *tc358746 = to_tc358746(sd);
 991
 992	if (pad != TC358746_SOURCE)
 993		return -EINVAL;
 994
 995	config->type = V4L2_MBUS_CSI2_DPHY;
 996	config->bus.mipi_csi2 = tc358746->csi_vep.bus.mipi_csi2;
 997
 998	return 0;
 999}
1000
1001static int __maybe_unused
1002tc358746_g_register(struct v4l2_subdev *sd, struct v4l2_dbg_register *reg)
1003{
1004	struct tc358746 *tc358746 = to_tc358746(sd);
1005	u32 val;
1006	int err;
1007
1008	/* 32-bit registers starting from CLW_DPHYCONTTX */
1009	reg->size = reg->reg < CLW_DPHYCONTTX_REG ? 2 : 4;
1010
1011	if (!pm_runtime_get_if_in_use(sd->dev))
1012		return 0;
1013
1014	err = tc358746_read(tc358746, reg->reg, &val);
1015	reg->val = val;
1016
1017	pm_runtime_put_sync_autosuspend(sd->dev);
1018
1019	return err;
1020}
1021
1022static int __maybe_unused
1023tc358746_s_register(struct v4l2_subdev *sd, const struct v4l2_dbg_register *reg)
1024{
1025	struct tc358746 *tc358746 = to_tc358746(sd);
1026
1027	if (!pm_runtime_get_if_in_use(sd->dev))
1028		return 0;
1029
1030	tc358746_write(tc358746, (u32)reg->reg, (u32)reg->val);
1031
1032	pm_runtime_put_sync_autosuspend(sd->dev);
1033
1034	return 0;
1035}
1036
1037static const struct v4l2_subdev_core_ops tc358746_core_ops = {
1038#ifdef CONFIG_VIDEO_ADV_DEBUG
1039	.g_register = tc358746_g_register,
1040	.s_register = tc358746_s_register,
1041#endif
1042};
1043
1044static const struct v4l2_subdev_video_ops tc358746_video_ops = {
1045	.s_stream = tc358746_s_stream,
1046};
1047
1048static const struct v4l2_subdev_pad_ops tc358746_pad_ops = {
1049	.enum_mbus_code = tc358746_enum_mbus_code,
1050	.set_fmt = tc358746_set_fmt,
1051	.get_fmt = v4l2_subdev_get_fmt,
1052	.link_validate = v4l2_subdev_link_validate_default,
1053	.get_mbus_config = tc358746_get_mbus_config,
1054};
1055
1056static const struct v4l2_subdev_ops tc358746_ops = {
1057	.core = &tc358746_core_ops,
1058	.video = &tc358746_video_ops,
1059	.pad = &tc358746_pad_ops,
1060};
1061
1062static const struct v4l2_subdev_internal_ops tc358746_internal_ops = {
1063	.init_state = tc358746_init_state,
1064};
1065
1066static const struct media_entity_operations tc358746_entity_ops = {
1067	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
1068	.link_validate = v4l2_subdev_link_validate,
1069};
1070
1071static int tc358746_mclk_enable(struct clk_hw *hw)
1072{
1073	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1074	unsigned int div;
1075	u32 val;
1076	int err;
1077
1078	div = tc358746->mclk_postdiv / 2;
1079	val = MCLK_HIGH(div - 1) | MCLK_LOW(div - 1);
1080	dev_dbg(tc358746->sd.dev, "MCLKCTL: %u (0x%x)\n", val, val);
1081	err = tc358746_write(tc358746, MCLKCTL_REG, val);
1082	if (err)
1083		return err;
1084
1085	if (tc358746->mclk_prediv == 8)
1086		val = MCLKDIV(MCLKDIV_8);
1087	else if (tc358746->mclk_prediv == 4)
1088		val = MCLKDIV(MCLKDIV_4);
1089	else
1090		val = MCLKDIV(MCLKDIV_2);
1091
1092	dev_dbg(tc358746->sd.dev, "CLKCTL[MCLKDIV]: %u (0x%x)\n", val, val);
1093
1094	return tc358746_update_bits(tc358746, CLKCTL_REG, MCLKDIV_MASK, val);
1095}
1096
1097static void tc358746_mclk_disable(struct clk_hw *hw)
1098{
1099	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1100
1101	tc358746_write(tc358746, MCLKCTL_REG, 0);
1102}
1103
1104static long
1105tc358746_find_mclk_settings(struct tc358746 *tc358746, unsigned long mclk_rate)
1106{
1107	unsigned long pll_rate = tc358746->pll_rate;
1108	const unsigned char prediv[] = { 2, 4, 8 };
1109	unsigned int mclk_prediv, mclk_postdiv;
1110	struct device *dev = tc358746->sd.dev;
1111	unsigned int postdiv, mclkdiv;
1112	unsigned long best_mclk_rate;
1113	unsigned int i;
1114
1115	/*
1116	 *                          MCLK-Div
1117	 *           -------------------´`---------------------
1118	 *          ´                                          `
1119	 *         +-------------+     +------------------------+
1120	 *         | MCLK-PreDiv |     |       MCLK-PostDiv     |
1121	 * PLL --> |   (2/4/8)   | --> | (mclk_low + mclk_high) | --> MCLK
1122	 *         +-------------+     +------------------------+
1123	 *
1124	 * The register value of mclk_low/high is mclk_low/high+1, i.e.:
1125	 *   mclk_low/high = 1   --> 2 MCLK-Ref Counts
1126	 *   mclk_low/high = 255 --> 256 MCLK-Ref Counts == max.
1127	 * If mclk_low and mclk_high are 0 then MCLK is disabled.
1128	 *
1129	 * Keep it simple and support 50/50 duty cycles only for now,
1130	 * so the calc will be:
1131	 *
1132	 *   MCLK = PLL / (MCLK-PreDiv * 2 * MCLK-PostDiv)
1133	 */
1134
1135	if (mclk_rate == tc358746->mclk_rate)
1136		return mclk_rate;
1137
1138	/* Highest possible rate */
1139	mclkdiv = pll_rate / mclk_rate;
1140	if (mclkdiv <= 8) {
1141		mclk_prediv = 2;
1142		mclk_postdiv = 4;
1143		best_mclk_rate = pll_rate / (2 * 4);
1144		goto out;
1145	}
1146
1147	/* First check the prediv */
1148	for (i = 0; i < ARRAY_SIZE(prediv); i++) {
1149		postdiv = mclkdiv / prediv[i];
1150
1151		if (postdiv % 2)
1152			continue;
1153
1154		if (postdiv >= 4 && postdiv <= 512) {
1155			mclk_prediv = prediv[i];
1156			mclk_postdiv = postdiv;
1157			best_mclk_rate = pll_rate / (prediv[i] * postdiv);
1158			goto out;
1159		}
1160	}
1161
1162	/* No suitable prediv found, so try to adjust the postdiv */
1163	for (postdiv = 4; postdiv <= 512; postdiv += 2) {
1164		unsigned int pre;
1165
1166		pre = mclkdiv / postdiv;
1167		if (pre == 2 || pre == 4 || pre == 8) {
1168			mclk_prediv = pre;
1169			mclk_postdiv = postdiv;
1170			best_mclk_rate = pll_rate / (pre * postdiv);
1171			goto out;
1172		}
1173	}
1174
1175	/* The MCLK <-> PLL gap is to high -> use largest possible div */
1176	mclk_prediv = 8;
1177	mclk_postdiv = 512;
1178	best_mclk_rate = pll_rate / (8 * 512);
1179
1180out:
1181	tc358746->mclk_prediv = mclk_prediv;
1182	tc358746->mclk_postdiv = mclk_postdiv;
1183	tc358746->mclk_rate = best_mclk_rate;
1184
1185	if (best_mclk_rate != mclk_rate)
1186		dev_warn(dev, "Request MCLK freq:%lu, found MCLK freq:%lu\n",
1187			 mclk_rate, best_mclk_rate);
1188
1189	dev_dbg(dev, "Found MCLK settings: freq:%lu prediv:%u postdiv:%u\n",
1190		best_mclk_rate, mclk_prediv, mclk_postdiv);
1191
1192	return best_mclk_rate;
1193}
1194
1195static unsigned long
1196tc358746_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1197{
1198	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1199	unsigned int prediv, postdiv;
1200	u32 val;
1201	int err;
1202
1203	err = tc358746_read(tc358746, MCLKCTL_REG, &val);
1204	if (err)
1205		return 0;
1206
1207	postdiv = FIELD_GET(MCLK_LOW_MASK, val) + 1;
1208	postdiv += FIELD_GET(MCLK_HIGH_MASK, val) + 1;
1209
1210	err = tc358746_read(tc358746, CLKCTL_REG, &val);
1211	if (err)
1212		return 0;
1213
1214	prediv = FIELD_GET(MCLKDIV_MASK, val);
1215	if (prediv == MCLKDIV_8)
1216		prediv = 8;
1217	else if (prediv == MCLKDIV_4)
1218		prediv = 4;
1219	else
1220		prediv = 2;
1221
1222	return tc358746->pll_rate / (prediv * postdiv);
1223}
1224
1225static int tc358746_mclk_determine_rate(struct clk_hw *hw,
1226					struct clk_rate_request *req)
1227{
1228	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1229
1230	req->best_parent_rate = tc358746->pll_rate;
1231
1232	req->rate = tc358746_find_mclk_settings(tc358746, req->rate);
1233
1234	return 0;
1235}
1236
1237static int tc358746_mclk_set_rate(struct clk_hw *hw, unsigned long rate,
1238				  unsigned long parent_rate)
1239{
1240	struct tc358746 *tc358746 = clk_hw_to_tc358746(hw);
1241
1242	tc358746_find_mclk_settings(tc358746, rate);
1243
1244	return tc358746_mclk_enable(hw);
1245}
1246
1247static const struct clk_ops tc358746_mclk_ops = {
1248	.enable = tc358746_mclk_enable,
1249	.disable = tc358746_mclk_disable,
1250	.recalc_rate = tc358746_recalc_rate,
1251	.determine_rate = tc358746_mclk_determine_rate,
1252	.set_rate = tc358746_mclk_set_rate,
1253};
1254
1255static int tc358746_setup_mclk_provider(struct tc358746 *tc358746)
1256{
1257	struct clk_init_data mclk_initdata = { };
1258	struct device *dev = tc358746->sd.dev;
1259	const char *mclk_name;
1260	int err;
1261
1262	/* MCLK clk provider support is optional */
1263	if (!device_property_present(dev, "#clock-cells"))
1264		return 0;
1265
1266	/* Init to highest possibel MCLK */
1267	tc358746->mclk_postdiv = 512;
1268	tc358746->mclk_prediv = 8;
1269
1270	mclk_name = "tc358746-mclk";
1271	device_property_read_string(dev, "clock-output-names", &mclk_name);
1272
1273	mclk_initdata.name = mclk_name;
1274	mclk_initdata.ops = &tc358746_mclk_ops;
1275	tc358746->mclk_hw.init = &mclk_initdata;
1276
1277	err = devm_clk_hw_register(dev, &tc358746->mclk_hw);
1278	if (err) {
1279		dev_err(dev, "Failed to register mclk provider\n");
1280		return err;
1281	}
1282
1283	err = devm_of_clk_add_hw_provider(dev, of_clk_hw_simple_get,
1284					  &tc358746->mclk_hw);
1285	if (err)
1286		dev_err(dev, "Failed to add mclk provider\n");
1287
1288	return err;
1289}
1290
1291static int
1292tc358746_init_subdev(struct tc358746 *tc358746, struct i2c_client *client)
1293{
1294	struct v4l2_subdev *sd = &tc358746->sd;
1295	int err;
1296
1297	v4l2_i2c_subdev_init(sd, client, &tc358746_ops);
1298	sd->internal_ops = &tc358746_internal_ops;
1299	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1300	sd->entity.function = MEDIA_ENT_F_VID_IF_BRIDGE;
1301	sd->entity.ops = &tc358746_entity_ops;
1302
1303	tc358746->pads[TC358746_SINK].flags = MEDIA_PAD_FL_SINK;
1304	tc358746->pads[TC358746_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
1305	err = media_entity_pads_init(&sd->entity, TC358746_NR_PADS,
1306				     tc358746->pads);
1307	if (err)
1308		return err;
1309
1310	err = v4l2_subdev_init_finalize(sd);
1311	if (err)
1312		media_entity_cleanup(&sd->entity);
1313
1314	return err;
1315}
1316
1317static int
1318tc358746_init_output_port(struct tc358746 *tc358746, unsigned long refclk)
1319{
1320	struct device *dev = tc358746->sd.dev;
1321	struct v4l2_fwnode_endpoint *vep;
1322	unsigned long csi_link_rate;
1323	struct fwnode_handle *ep;
1324	unsigned char csi_lanes;
1325	int err;
1326
1327	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(dev), TC358746_SOURCE,
1328					     0, 0);
1329	if (!ep) {
1330		dev_err(dev, "Missing endpoint node\n");
1331		return -EINVAL;
1332	}
1333
1334	/* Currently we only support 'parallel in' -> 'csi out' */
1335	vep = &tc358746->csi_vep;
1336	vep->bus_type = V4L2_MBUS_CSI2_DPHY;
1337	err = v4l2_fwnode_endpoint_alloc_parse(ep, vep);
1338	fwnode_handle_put(ep);
1339	if (err) {
1340		dev_err(dev, "Failed to parse source endpoint\n");
1341		return err;
1342	}
1343
1344	csi_lanes = vep->bus.mipi_csi2.num_data_lanes;
1345	if (csi_lanes == 0 || csi_lanes > 4 ||
1346	    vep->nr_of_link_frequencies == 0) {
1347		dev_err(dev, "error: Invalid CSI-2 settings\n");
1348		err = -EINVAL;
1349		goto err;
1350	}
1351
1352	/* TODO: Add support to handle multiple link frequencies */
1353	csi_link_rate = (unsigned long)vep->link_frequencies[0];
1354	tc358746->pll_rate = tc358746_find_pll_settings(tc358746, refclk,
1355							csi_link_rate * 2);
1356	if (!tc358746->pll_rate) {
1357		err = -EINVAL;
1358		goto err;
1359	}
1360
1361	err = phy_mipi_dphy_get_default_config_for_hsclk(tc358746->pll_rate,
1362						csi_lanes, &tc358746->dphy_cfg);
1363	if (err)
1364		goto err;
1365
1366	return 0;
1367
1368err:
1369	v4l2_fwnode_endpoint_free(vep);
1370
1371	return err;
1372}
1373
1374static int tc358746_init_hw(struct tc358746 *tc358746)
1375{
1376	struct device *dev = tc358746->sd.dev;
1377	unsigned int chipid;
1378	u32 val;
1379	int err;
1380
1381	err = pm_runtime_resume_and_get(dev);
1382	if (err < 0) {
1383		dev_err(dev, "Failed to resume the device\n");
1384		return err;
1385	}
1386
1387	 /* Ensure that CSI interface is put into LP-11 state */
1388	err = tc358746_sw_reset(tc358746);
1389	if (err) {
1390		pm_runtime_put_sync(dev);
1391		dev_err(dev, "Failed to reset the device\n");
1392		return err;
1393	}
1394
1395	err = tc358746_read(tc358746, CHIPID_REG, &val);
1396	pm_runtime_put_sync_autosuspend(dev);
1397	if (err)
1398		return -ENODEV;
1399
1400	chipid = FIELD_GET(CHIPID, val);
1401	if (chipid != 0x44) {
1402		dev_err(dev, "Invalid chipid 0x%02x\n", chipid);
1403		return -ENODEV;
1404	}
1405
1406	return 0;
1407}
1408
1409static int tc358746_init_controls(struct tc358746 *tc358746)
1410{
1411	u64 *link_frequencies = tc358746->csi_vep.link_frequencies;
1412	struct v4l2_ctrl *ctrl;
1413	int err;
1414
1415	err = v4l2_ctrl_handler_init(&tc358746->ctrl_hdl, 1);
1416	if (err)
1417		return err;
1418
1419	/*
1420	 * The driver currently supports only one link-frequency, regardless of
1421	 * the input from the firmware, see: tc358746_init_output_port(). So
1422	 * report only the first frequency from the array of possible given
1423	 * frequencies.
1424	 */
1425	ctrl = v4l2_ctrl_new_int_menu(&tc358746->ctrl_hdl, NULL,
1426				      V4L2_CID_LINK_FREQ, 0, 0,
1427				      link_frequencies);
1428	if (ctrl)
1429		ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
1430
1431	err = tc358746->ctrl_hdl.error;
1432	if (err) {
1433		v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1434		return err;
1435	}
1436
1437	tc358746->sd.ctrl_handler = &tc358746->ctrl_hdl;
1438
1439	return 0;
1440}
1441
1442static int tc358746_notify_bound(struct v4l2_async_notifier *notifier,
1443				 struct v4l2_subdev *sd,
1444				 struct v4l2_async_connection *asd)
1445{
1446	struct tc358746 *tc358746 =
1447		container_of(notifier, struct tc358746, notifier);
1448	u32 flags = MEDIA_LNK_FL_ENABLED | MEDIA_LNK_FL_IMMUTABLE;
1449	struct media_pad *sink = &tc358746->pads[TC358746_SINK];
1450
1451	return v4l2_create_fwnode_links_to_pad(sd, sink, flags);
1452}
1453
1454static const struct v4l2_async_notifier_operations tc358746_notify_ops = {
1455	.bound = tc358746_notify_bound,
1456};
1457
1458static int tc358746_async_register(struct tc358746 *tc358746)
1459{
1460	struct v4l2_fwnode_endpoint vep = {
1461		.bus_type = V4L2_MBUS_PARALLEL,
1462	};
1463	struct v4l2_async_connection *asd;
1464	struct fwnode_handle *ep;
1465	int err;
1466
1467	ep = fwnode_graph_get_endpoint_by_id(dev_fwnode(tc358746->sd.dev),
1468					     TC358746_SINK, 0, 0);
1469	if (!ep)
1470		return -ENOTCONN;
1471
1472	err = v4l2_fwnode_endpoint_parse(ep, &vep);
1473	if (err) {
1474		fwnode_handle_put(ep);
1475		return err;
1476	}
1477
1478	v4l2_async_subdev_nf_init(&tc358746->notifier, &tc358746->sd);
1479	asd = v4l2_async_nf_add_fwnode_remote(&tc358746->notifier, ep,
1480					      struct v4l2_async_connection);
1481	fwnode_handle_put(ep);
1482
1483	if (IS_ERR(asd)) {
1484		err = PTR_ERR(asd);
1485		goto err_cleanup;
1486	}
1487
1488	tc358746->notifier.ops = &tc358746_notify_ops;
1489
1490	err = v4l2_async_nf_register(&tc358746->notifier);
1491	if (err)
1492		goto err_cleanup;
1493
1494	err = v4l2_async_register_subdev(&tc358746->sd);
1495	if (err)
1496		goto err_unregister;
1497
1498	return 0;
1499
1500err_unregister:
1501	v4l2_async_nf_unregister(&tc358746->notifier);
1502err_cleanup:
1503	v4l2_async_nf_cleanup(&tc358746->notifier);
1504
1505	return err;
1506}
1507
1508static int tc358746_probe(struct i2c_client *client)
1509{
1510	struct device *dev = &client->dev;
1511	struct tc358746 *tc358746;
1512	unsigned long refclk;
1513	unsigned int i;
1514	int err;
1515
1516	tc358746 = devm_kzalloc(&client->dev, sizeof(*tc358746), GFP_KERNEL);
1517	if (!tc358746)
1518		return -ENOMEM;
1519
1520	tc358746->regmap = devm_regmap_init_i2c(client, &tc358746_regmap_config);
1521	if (IS_ERR(tc358746->regmap))
1522		return dev_err_probe(dev, PTR_ERR(tc358746->regmap),
1523				     "Failed to init regmap\n");
1524
1525	tc358746->refclk = devm_clk_get(dev, "refclk");
1526	if (IS_ERR(tc358746->refclk))
1527		return dev_err_probe(dev, PTR_ERR(tc358746->refclk),
1528				     "Failed to get refclk\n");
1529
1530	err = clk_prepare_enable(tc358746->refclk);
1531	if (err)
1532		return dev_err_probe(dev, err,
1533				     "Failed to enable refclk\n");
1534
1535	refclk = clk_get_rate(tc358746->refclk);
1536	clk_disable_unprepare(tc358746->refclk);
1537
1538	if (refclk < 6 * HZ_PER_MHZ || refclk > 40 * HZ_PER_MHZ)
1539		return dev_err_probe(dev, -EINVAL, "Invalid refclk range\n");
1540
1541	for (i = 0; i < ARRAY_SIZE(tc358746_supplies); i++)
1542		tc358746->supplies[i].supply = tc358746_supplies[i];
1543
1544	err = devm_regulator_bulk_get(dev, ARRAY_SIZE(tc358746_supplies),
1545				      tc358746->supplies);
1546	if (err)
1547		return dev_err_probe(dev, err, "Failed to get supplies\n");
1548
1549	tc358746->reset_gpio = devm_gpiod_get_optional(dev, "reset",
1550						       GPIOD_OUT_HIGH);
1551	if (IS_ERR(tc358746->reset_gpio))
1552		return dev_err_probe(dev, PTR_ERR(tc358746->reset_gpio),
1553				     "Failed to get reset-gpios\n");
1554
1555	err = tc358746_init_subdev(tc358746, client);
1556	if (err)
1557		return dev_err_probe(dev, err, "Failed to init subdev\n");
1558
1559	err = tc358746_init_output_port(tc358746, refclk);
1560	if (err)
1561		goto err_subdev;
1562
1563	/*
1564	 * Keep this order since we need the output port link-frequencies
1565	 * information.
1566	 */
1567	err = tc358746_init_controls(tc358746);
1568	if (err)
1569		goto err_fwnode;
1570
1571	dev_set_drvdata(dev, tc358746);
1572
1573	/* Set to 1sec to give the stream reconfiguration enough time */
1574	pm_runtime_set_autosuspend_delay(dev, 1000);
1575	pm_runtime_use_autosuspend(dev);
1576	pm_runtime_enable(dev);
1577
1578	err = tc358746_init_hw(tc358746);
1579	if (err)
1580		goto err_pm;
1581
1582	err = tc358746_setup_mclk_provider(tc358746);
1583	if (err)
1584		goto err_pm;
1585
1586	err = tc358746_async_register(tc358746);
1587	if (err < 0)
1588		goto err_pm;
1589
1590	dev_dbg(dev, "%s found @ 0x%x (%s)\n", client->name,
1591		client->addr, client->adapter->name);
1592
1593	return 0;
1594
1595err_pm:
1596	pm_runtime_disable(dev);
1597	pm_runtime_set_suspended(dev);
1598	pm_runtime_dont_use_autosuspend(dev);
1599	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1600err_fwnode:
1601	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1602err_subdev:
1603	v4l2_subdev_cleanup(&tc358746->sd);
1604	media_entity_cleanup(&tc358746->sd.entity);
1605
1606	return err;
1607}
1608
1609static void tc358746_remove(struct i2c_client *client)
1610{
1611	struct v4l2_subdev *sd = i2c_get_clientdata(client);
1612	struct tc358746 *tc358746 = to_tc358746(sd);
1613
1614	v4l2_subdev_cleanup(sd);
1615	v4l2_ctrl_handler_free(&tc358746->ctrl_hdl);
1616	v4l2_fwnode_endpoint_free(&tc358746->csi_vep);
1617	v4l2_async_nf_unregister(&tc358746->notifier);
1618	v4l2_async_nf_cleanup(&tc358746->notifier);
1619	v4l2_async_unregister_subdev(sd);
1620	media_entity_cleanup(&sd->entity);
1621
1622	pm_runtime_disable(sd->dev);
1623	pm_runtime_set_suspended(sd->dev);
1624	pm_runtime_dont_use_autosuspend(sd->dev);
1625}
1626
1627/*
1628 * This function has been created just to avoid a smatch warning,
1629 * please do not merge it into tc358746_suspend until you have
1630 * confirmed that it does not introduce a new warning.
1631 */
1632static void tc358746_clk_enable(struct tc358746 *tc358746)
1633{
1634	clk_prepare_enable(tc358746->refclk);
1635}
1636
1637static int tc358746_suspend(struct device *dev)
1638{
1639	struct tc358746 *tc358746 = dev_get_drvdata(dev);
1640	int err;
1641
1642	clk_disable_unprepare(tc358746->refclk);
1643
1644	err = regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1645				     tc358746->supplies);
1646	if (err)
1647		tc358746_clk_enable(tc358746);
1648
1649	return err;
1650}
1651
1652static int tc358746_resume(struct device *dev)
1653{
1654	struct tc358746 *tc358746 = dev_get_drvdata(dev);
1655	int err;
1656
1657	gpiod_set_value(tc358746->reset_gpio, 1);
1658
1659	err = regulator_bulk_enable(ARRAY_SIZE(tc358746_supplies),
1660				    tc358746->supplies);
1661	if (err)
1662		return err;
1663
1664	/* min. 200ns */
1665	usleep_range(10, 20);
1666
1667	gpiod_set_value(tc358746->reset_gpio, 0);
1668
1669	err = clk_prepare_enable(tc358746->refclk);
1670	if (err)
1671		goto err;
1672
1673	/* min. 700us ... 1ms */
1674	usleep_range(1000, 1500);
1675
1676	/*
1677	 * Enable the PLL here since it can be called by the clk-framework or by
1678	 * the .s_stream() callback. So this is the common place for both.
1679	 */
1680	err = tc358746_apply_pll_config(tc358746);
1681	if (err)
1682		goto err_clk;
1683
1684	return 0;
1685
1686err_clk:
1687	clk_disable_unprepare(tc358746->refclk);
1688err:
1689	regulator_bulk_disable(ARRAY_SIZE(tc358746_supplies),
1690			       tc358746->supplies);
1691	return err;
1692}
1693
1694static DEFINE_RUNTIME_DEV_PM_OPS(tc358746_pm_ops, tc358746_suspend,
1695				 tc358746_resume, NULL);
1696
1697static const struct of_device_id __maybe_unused tc358746_of_match[] = {
1698	{ .compatible = "toshiba,tc358746" },
1699	{ },
1700};
1701MODULE_DEVICE_TABLE(of, tc358746_of_match);
1702
1703static struct i2c_driver tc358746_driver = {
1704	.driver = {
1705		.name = "tc358746",
1706		.pm = pm_ptr(&tc358746_pm_ops),
1707		.of_match_table = tc358746_of_match,
1708	},
1709	.probe = tc358746_probe,
1710	.remove = tc358746_remove,
1711};
1712
1713module_i2c_driver(tc358746_driver);
1714
1715MODULE_DESCRIPTION("Toshiba TC358746 Parallel to CSI-2 bridge driver");
1716MODULE_AUTHOR("Marco Felsch <kernel@pengutronix.de>");
1717MODULE_LICENSE("GPL");