3 files changed, 37 insertions, 36 deletions

diff --git a/src/soc/mediatek/common/dsi.c b/src/soc/mediatek/common/dsi.c
index 3d003ec94f..92a645682e 100644
--- a/src/soc/mediatek/common/dsi.c
+++ b/src/soc/mediatek/common/dsi.c
@@ -21,6 +21,22 @@
 #include <soc/dsi.h>
 #include <timer.h>
 
+static unsigned int mtk_dsi_get_bits_per_pixel(u32 format)
+{
+	switch (format) {
+	case MIPI_DSI_FMT_RGB565:
+		return 16;
+	case MIPI_DSI_FMT_RGB666:
+	case MIPI_DSI_FMT_RGB666_PACKED:
+		return 18;
+	case MIPI_DSI_FMT_RGB888:
+		return 24;
+	}
+	printk(BIOS_WARNING, "%s: WARN: Unknown format %d, assuming 24 bpp\n",
+	       __func__, format);
+	return 24;
+}
+
 static void mtk_dsi_phy_timconfig(u32 data_rate)
 {
 	u32 timcon0, timcon1, timcon2, timcon3;
@@ -106,15 +122,11 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 	u32 hfp_byte;
 	u32 vbp_byte;
 	u32 vfp_byte;
-	u32 bpp;
+	u32 bytes_per_pixel;
 	u32 packet_fmt;
 	u32 hactive;
 
-	if (format == MIPI_DSI_FMT_RGB565)
-		bpp = 2;
-	else
-		bpp = 3;
-
+	bytes_per_pixel = DIV_ROUND_UP(mtk_dsi_get_bits_per_pixel(format), 8);
 	vbp_byte = edid->mode.vbl - edid->mode.vso - edid->mode.vspw -
 		   edid->mode.vborder;
 	vfp_byte = edid->mode.vso - edid->mode.vborder;
@@ -126,13 +138,13 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 
 	if (mode_flags & MIPI_DSI_MODE_VIDEO_SYNC_PULSE)
 		hbp_byte = (edid->mode.hbl - edid->mode.hso - edid->mode.hspw -
-			    edid->mode.hborder) * bpp - 10;
+			    edid->mode.hborder) * bytes_per_pixel - 10;
 	else
 		hbp_byte = (edid->mode.hbl - edid->mode.hso -
-			    edid->mode.hborder) * bpp - 10;
+			    edid->mode.hborder) * bytes_per_pixel - 10;
 
-	hsync_active_byte = edid->mode.hspw * bpp - 10;
-	hfp_byte = (edid->mode.hso - edid->mode.hborder) * bpp - 12;
+	hsync_active_byte = edid->mode.hspw * bytes_per_pixel - 10;
+	hfp_byte = (edid->mode.hso - edid->mode.hborder) * bytes_per_pixel - 12;
 
 	write32(&dsi0->dsi_hsa_wc, hsync_active_byte);
 	write32(&dsi0->dsi_hbp_wc, hbp_byte);
@@ -157,7 +169,7 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 	}
 
 	hactive = edid->mode.ha;
-	packet_fmt |= (hactive * bpp) & DSI_PS_WC;
+	packet_fmt |= (hactive * bytes_per_pixel) & DSI_PS_WC;
 
 	write32(&dsi0->dsi_psctrl, packet_fmt);
 }
@@ -172,8 +184,9 @@ static void mtk_dsi_start(void)
 int mtk_dsi_init(u32 mode_flags, u32 format, u32 lanes, const struct edid *edid)
 {
 	int data_rate;
+	u32 bits_per_pixel = mtk_dsi_get_bits_per_pixel(format);
 
-	data_rate = mtk_dsi_phy_clk_setting(format, lanes, edid);
+	data_rate = mtk_dsi_phy_clk_setting(bits_per_pixel, lanes, edid);
 
 	if (data_rate < 0)
 		return -1;
diff --git a/src/soc/mediatek/common/include/soc/dsi_common.h b/src/soc/mediatek/common/include/soc/dsi_common.h
index cebf9af701..7a48d53507 100644
--- a/src/soc/mediatek/common/include/soc/dsi_common.h
+++ b/src/soc/mediatek/common/include/soc/dsi_common.h
@@ -303,7 +303,8 @@ enum {
 /* Functions that each SOC should provide. */
 void mtk_dsi_reset(void);
 /* mtk_dsi_phy_clk_setting should return the data rate in Mbps. */
-int mtk_dsi_phy_clk_setting(u32 format, u32 lanes, const struct edid *edid);
+int mtk_dsi_phy_clk_setting(u32 bits_per_pixel, u32 lanes,
+			    const struct edid *edid);
 
 /* Public API provided in common/dsi.c */
 int mtk_dsi_init(u32 mode_flags, u32 format, u32 lanes,
diff --git a/src/soc/mediatek/mt8173/dsi.c b/src/soc/mediatek/mt8173/dsi.c
index e2e843e19d..25030c621c 100644
--- a/src/soc/mediatek/mt8173/dsi.c
+++ b/src/soc/mediatek/mt8173/dsi.c
@@ -20,12 +20,12 @@
 #include <soc/dsi.h>
 #include <timer.h>
 
-int mtk_dsi_phy_clk_setting(u32 format, u32 lanes, const struct edid *edid)
+int mtk_dsi_phy_clk_setting(u32 bits_per_pixel, u32 lanes,
+			    const struct edid *edid)
 {
 	u32 txdiv0, txdiv1;
 	u64 pcw;
 	u32 reg;
-	u32 bit_per_pixel;
 	int i, data_rate, mipi_tx_rate;
 
 	reg = read32(&mipi_tx0->dsi_bg_con);
@@ -52,29 +52,14 @@ int mtk_dsi_phy_clk_setting(u32 format, u32 lanes, const struct edid *edid)
 
 	clrbits_le32(&mipi_tx0->dsi_pll_con0, RG_DSI0_MPPLL_PLL_EN);
 
-	switch (format) {
-	case MIPI_DSI_FMT_RGB565:
-		bit_per_pixel = 16;
-		break;
-	case MIPI_DSI_FMT_RGB666:
-	case MIPI_DSI_FMT_RGB666_PACKED:
-		bit_per_pixel = 18;
-		break;
-	case MIPI_DSI_FMT_RGB888:
-	default:
-		bit_per_pixel = 24;
-		break;
-	}
-
 	/**
-	 * data_rate = (pixel_clock / 1000) * bit_per_pixel * mipi_ratio / lane_num
+	 * data_rate = pixel_clock / 1000 * bits_per_pixel * mipi_ratio / lanes
 	 * pixel_clock unit is Khz, data_rata unit is MHz, so need divide 1000.
 	 * mipi_ratio is mipi clk coefficient for balance the pixel clk in mipi.
 	 * we set mipi_ratio is 1.02.
-	 * lane_num
 	 */
-	data_rate = edid->mode.pixel_clock * 102 * bit_per_pixel /
-			 (lanes * 1000 * 100);
+	data_rate = edid->mode.pixel_clock * 102 * bits_per_pixel /
+			(lanes * 1000 * 100);
 	mipi_tx_rate = data_rate;
 
 	if (data_rate > 500) {
@@ -93,9 +78,11 @@ int mtk_dsi_phy_clk_setting(u32 format, u32 lanes, const struct edid *edid)
 		txdiv0 = 2;
 		txdiv1 = 2;
 	} else {
-		printk(BIOS_ERR, "data rate (%u) must be >=50. Please check "
-		       "pixel clock (%u), bpp (%u), and number of lanes (%u)\n",
-		       data_rate, edid->mode.pixel_clock, bit_per_pixel, lanes);
+		printk(BIOS_ERR, "data rate (%u) must be >=50. "
+		       "Please check pixel clock (%u), bits per pixel (%u), "
+		       "and number of lanes (%u)\n",
+		       data_rate, edid->mode.pixel_clock, bits_per_pixel,
+		       lanes);
 		return -1;
 	}