video/drm: dsi: fix pll clock setting for synopsys phy

Change-Id: Ib2cfdf413e3c4da039a16971fcc00baaab3b101c
Signed-off-by: Wyon Bi <bivvy.bi@rock-chips.com>
(sync from rk/kernel:develop-4.4:331690a183f536fe8a791ceed4231f7e484f8fb7)

drivers/video/drm/rockchip-dw-mipi-dsi.c

@@ -229,6 +229,7 @@ struct mipi_dphy {
 };
 
 struct dw_mipi_dsi {
+	struct udevice *dev;
 	void *base;
 	void *grf;
 	const void *blob;
@@ -468,6 +469,9 @@ static int mipi_dphy_power_on(struct dw_mipi_dsi *dsi)
 	mipi_dphy_rstz_deassert(dsi);
 	mdelay(2);
 
+	if (dsi->dphy.phy)
+		rockchip_phy_power_on(dsi->dphy.phy);
+
 	ret = readl_poll_timeout(dsi->base + DSI_PHY_STATUS,
 				 val, val & PHY_LOCK,
 				 1000, PHY_STATUS_TIMEOUT_US);
@@ -537,76 +541,106 @@ static void dw_mipi_dsi_phy_init(struct dw_mipi_dsi *dsi)
 	testif_write(dsi, 0x18, FEEDBACK_DIV_HI(m >> 5));
 }
 
-static unsigned long dw_mipi_dsi_calc_bandwidth(struct dw_mipi_dsi *dsi)
+static unsigned long dw_mipi_dsi_get_lane_rate(struct dw_mipi_dsi *dsi)
 {
-	int bpp;
+	const struct drm_display_mode *mode = dsi->mode;
-	unsigned long mpclk, tmp;
+	unsigned long max_lane_rate = dsi->pdata->max_bit_rate_per_lane;
-	unsigned long target_mbps = 1000;
+	unsigned long lane_rate;
-	unsigned int max_mbps;
+	unsigned int value;
-	int lanes;
+	int bpp, lanes;
-	int rate;
+	u64 tmp;
 
 	/* optional override of the desired bandwidth */
-	rate = ofnode_read_u32_default(dsi->node, "rockchip,lane-rate", -1);
+	value = dev_read_u32_default(dsi->dev, "rockchip,lane-rate", 0);
-	if (rate > 0) {
+	if (value > 0)
-		return rate;
+		return value * USEC_PER_SEC;
-	}
-	max_mbps = dsi->pdata->max_bit_rate_per_lane / USEC_PER_SEC;
 
 	bpp = mipi_dsi_pixel_format_to_bpp(dsi->format);
-	if (bpp < 0) {
+	if (bpp < 0)
-		printf("failed to get bpp for pixel format %d\n",
-			dsi->format);
 		bpp = 24;
-	}
 
 	lanes = dsi->slave ? dsi->lanes * 2 : dsi->lanes;
+	tmp = (u64)mode->clock * 1000 * bpp;
+	do_div(tmp, lanes);
 
-	mpclk = DIV_ROUND_UP(dsi->mode->clock, MSEC_PER_SEC);
-	if (mpclk) {
 	/* take 1 / 0.9, since mbps must big than bandwidth of RGB */
-		tmp = mpclk * (bpp / lanes) * 10 / 9;
+	tmp *= 10;
-		if (tmp < max_mbps)
+	do_div(tmp, 9);
-			target_mbps = tmp;
-		else
-			printf("DPHY clock frequency is out of range\n");
-	}
 
-	return target_mbps;
+	if (tmp > max_lane_rate)
+		lane_rate = max_lane_rate;
+	else
+		lane_rate = tmp;
+
+	return lane_rate;
 }
 
-static int dw_mipi_dsi_get_lane_bps(struct dw_mipi_dsi *dsi)
+static void dw_mipi_dsi_set_pll(struct dw_mipi_dsi *dsi, unsigned long rate)
 {
-	unsigned int i, pre;
+	unsigned long fin, fout;
-	unsigned long pllref, tmp;
+	unsigned long fvco_min, fvco_max, best_freq = 984000000;
-	unsigned int m = 1, n = 1;
+	u8 min_prediv, max_prediv;
-	unsigned long target_mbps;
+	u8 _prediv, best_prediv = 2;
+	u16 _fbdiv, best_fbdiv = 82;
+	u32 min_delta = ~0U;
 
-	if (dsi->master)
+	fin = 24000000;
-		return 0;
+	fout = rate;
 
-	target_mbps = dw_mipi_dsi_calc_bandwidth(dsi);
+	/* 5Mhz < Fref / N < 40MHz, 80MHz < Fvco < 1500Mhz */
+	min_prediv = DIV_ROUND_UP(fin, 40000000);
+	max_prediv = fin / 5000000;
+	fvco_min = 80000000;
+	fvco_max = 1500000000;
 
-	/* ref clk : 24MHz*/
+	for (_prediv = min_prediv; _prediv <= max_prediv; _prediv++) {
-	pllref = 24;
+		u64 tmp, _fout;
-	tmp = pllref;
+		u32 delta;
 
-	for (i = 1; i < 6; i++) {
+		/* Fvco = Fref * M / N */
-		pre = pllref / i;
+		tmp = (u64)fout * _prediv;
-		if ((tmp > (target_mbps % pre)) && (target_mbps / pre < 512)) {
+		do_div(tmp, fin);
-			tmp = target_mbps % pre;
+		_fbdiv = tmp;
-			n = i;
+
-			m = target_mbps / pre;
+		/*
-		}
+		 * Due to the use of a "by 2 pre-scaler," the range of the
-		if (tmp == 0)
+		 * feedback multiplication value M is limited to even division
+		 * numbers, and m must be greater than 12, less than 1000.
+		 */
+		if (_fbdiv <= 12 || _fbdiv >= 1000)
+			continue;
+
+		if (_fbdiv % 2)
+			++_fbdiv;
+
+		_fout = (u64)_fbdiv * fin;
+		do_div(_fout, _prediv);
+
+		if (_fout < fvco_min || _fout > fvco_max)
+			continue;
+
+		delta = abs(fout - _fout);
+		if (!delta) {
+			best_prediv = _prediv;
+			best_fbdiv = _fbdiv;
+			best_freq = _fout;
 			break;
+		} else if (delta < min_delta) {
+			best_prediv = _prediv;
+			best_fbdiv = _fbdiv;
+			best_freq = _fout;
+			min_delta = delta;
+		}
 	}
 
-	dsi->lane_mbps = pllref / n * m;
+	dsi->lane_mbps = best_freq / USEC_PER_SEC;
-	dsi->dphy.input_div = n;
+	dsi->dphy.input_div = best_prediv;
-	dsi->dphy.feedback_div = m;
+	dsi->dphy.feedback_div = best_fbdiv;
-
+	if (dsi->slave) {
-	return 0;
+		dsi->slave->lane_mbps = dsi->lane_mbps;
+		dsi->slave->dphy.input_div = dsi->dphy.input_div;
+		dsi->slave->dphy.feedback_div = dsi->dphy.feedback_div;
+	}
 }
 
 static int dw_mipi_dsi_read_from_fifo(struct dw_mipi_dsi *dsi,
@@ -1108,16 +1142,11 @@ static int dw_mipi_dsi_connector_init(struct display_state *state)
 	const struct rockchip_connector *connector = conn_state->connector;
 	const struct dw_mipi_dsi_plat_data *pdata = connector->data;
 	ofnode mipi_node = conn_state->node;
-	struct dw_mipi_dsi *dsi;
+	struct dw_mipi_dsi *dsi = dev_get_priv(conn_state->dev);
 	ofnode panel;
 	int id;
 	int ret;
 
-	dsi = malloc(sizeof(*dsi));
-	if (!dsi)
-		return -ENOMEM;
-	memset(dsi, 0, sizeof(*dsi));
-
 	dsi->base = dev_read_addr_ptr(conn_state->dev);
 	dsi->grf = syscon_get_first_range(ROCKCHIP_SYSCON_GRF);
 	if (dsi->grf <= 0) {
@@ -1169,29 +1198,10 @@ static int dw_mipi_dsi_connector_init(struct display_state *state)
 	return 0;
 }
 
-static void dw_mipi_dsi_pre_init(struct dw_mipi_dsi *dsi)
+static void dw_mipi_dsi_set_hs_clk(struct dw_mipi_dsi *dsi, unsigned long rate)
 {
-	unsigned long bw, rate;
+	rate = rockchip_phy_set_pll(dsi->dphy.phy, rate);
-
-	if (dsi->dphy.phy) {
-		bw = dw_mipi_dsi_calc_bandwidth(dsi);
-		rate = rockchip_phy_set_pll(dsi->dphy.phy, bw * USEC_PER_SEC);
 	dsi->lane_mbps = rate / USEC_PER_SEC;
-		rockchip_phy_set_mode(dsi->dphy.phy, PHY_MODE_VIDEO_MIPI);
-		rockchip_phy_power_on(dsi->dphy.phy);
-	} else {
-		dw_mipi_dsi_get_lane_bps(dsi);
-	}
-
-	if (dsi->slave) {
-		dsi->slave->mode = dsi->mode;
-		dsi->slave->lane_mbps = dsi->lane_mbps;
-		dsi->slave->dphy.input_div = dsi->dphy.input_div;
-		dsi->slave->dphy.feedback_div = dsi->dphy.feedback_div;
-	}
-
-	printf("final DSI-Link bandwidth: %u Mbps x %d\n",
-	       dsi->lane_mbps, dsi->lanes);
 }
 
 static void dw_mipi_dsi_host_init(struct dw_mipi_dsi *dsi)
@@ -1270,11 +1280,22 @@ static int dw_mipi_dsi_connector_prepare(struct display_state *state)
 	struct connector_state *conn_state = &state->conn_state;
 	struct crtc_state *crtc_state = &state->crtc_state;
 	struct dw_mipi_dsi *dsi = conn_state->private;
+	unsigned long lane_rate;
 
 	dsi->mode = &conn_state->mode;
+	if (dsi->slave)
+		dsi->slave->mode = dsi->mode;
+
+	lane_rate = dw_mipi_dsi_get_lane_rate(dsi);
+	if (dsi->dphy.phy)
+		dw_mipi_dsi_set_hs_clk(dsi, lane_rate);
+	else
+		dw_mipi_dsi_set_pll(dsi, lane_rate);
+
+	printf("final DSI-Link bandwidth: %u Mbps x %d\n",
+	       dsi->lane_mbps, dsi->slave ? dsi->lanes * 2 : dsi->lanes);
 
 	dw_mipi_dsi_vop_routing(dsi, crtc_state->crtc_id);
-	dw_mipi_dsi_pre_init(dsi);
 	dw_mipi_dsi_pre_enable(dsi);
 
 	return 0;
@@ -1519,6 +1540,10 @@ static const struct udevice_id dw_mipi_dsi_ids[] = {
 
 static int dw_mipi_dsi_probe(struct udevice *dev)
 {
+	struct dw_mipi_dsi *dsi = dev_get_priv(dev);
+
+	dsi->dev = dev;
+
 	return 0;
 }