4 files changed, 37 insertions, 11 deletions

diff --git a/src/soc/mediatek/common/dsi.c b/src/soc/mediatek/common/dsi.c
index 1a768a59a5..392b02d24d 100644
--- a/src/soc/mediatek/common/dsi.c
+++ b/src/soc/mediatek/common/dsi.c
@@ -170,8 +170,9 @@ static void mtk_dsi_rxtx_control(u32 mode_flags, u32 lanes)
 	write32(&dsi0->dsi_txrx_ctrl, tmp_reg);
 }
 
-static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
-				      const struct edid *edid)
+static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format, u32 lanes,
+				      const struct edid *edid,
+				      const struct mtk_phy_timing *phy_timing)
 {
 	u32 hsync_active_byte;
 	u32 hbp_byte;
@@ -181,6 +182,7 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 	u32 bytes_per_pixel;
 	u32 packet_fmt;
 	u32 hactive;
+	u32 data_phy_cycles;
 
 	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 -
@@ -192,15 +194,30 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 	write32(&dsi0->dsi_vfp_nl, vfp_byte);
 	write32(&dsi0->dsi_vact_nl, edid->mode.va);
 
+	unsigned int hspw = 0;
 	if (mode_flags & MIPI_DSI_MODE_VIDEO_SYNC_PULSE)
-		hbp_byte = (edid->mode.hbl - edid->mode.hso - edid->mode.hspw -
-			    edid->mode.hborder) * bytes_per_pixel - 10;
-	else
-		hbp_byte = (edid->mode.hbl - edid->mode.hso -
-			    edid->mode.hborder) * bytes_per_pixel - 10;
+		hspw = edid->mode.hspw;
 
+	hbp_byte = (edid->mode.hbl - edid->mode.hso - hspw - edid->mode.hborder)
+			* bytes_per_pixel - 10;
 	hsync_active_byte = edid->mode.hspw * bytes_per_pixel - 10;
-	hfp_byte = (edid->mode.hso - edid->mode.hborder) * bytes_per_pixel - 12;
+	hfp_byte = (edid->mode.hso - edid->mode.hborder) * bytes_per_pixel;
+
+	data_phy_cycles = phy_timing->lpx + phy_timing->da_hs_prepare +
+		phy_timing->da_hs_zero + phy_timing->da_hs_exit + 2;
+
+	u32 delta = 12;
+	if (mode_flags & MIPI_DSI_MODE_VIDEO_BURST)
+		delta += 6;
+
+	u32 d_phy = phy_timing->d_phy;
+	if (d_phy == 0)
+		d_phy = data_phy_cycles * lanes + delta;
+	if (hfp_byte > d_phy)
+		hfp_byte -= d_phy;
+	else
+		printk(BIOS_ERR, "HFP is not greater than d-phy, FPS < 60Hz "
+		       "and the panel may not work properly.\n");
 
 	write32(&dsi0->dsi_hsa_wc, hsync_active_byte);
 	write32(&dsi0->dsi_hbp_wc, hbp_byte);
@@ -227,7 +244,9 @@ static void mtk_dsi_config_vdo_timing(u32 mode_flags, u32 format,
 	hactive = edid->mode.ha;
 	packet_fmt |= (hactive * bytes_per_pixel) & DSI_PS_WC;
 
-	write32(&dsi0->dsi_psctrl, packet_fmt);
+	write32(&dsi0->dsi_psctrl,
+		PIXEL_STREAM_CUSTOM_HEADER << DSI_PSCON_CUSTOM_HEADER_SHIFT |
+		packet_fmt);
 }
 
 static void mtk_dsi_start(void)
@@ -252,7 +271,7 @@ int mtk_dsi_init(u32 mode_flags, u32 format, u32 lanes, const struct edid *edid)
 	mtk_dsi_phy_timing(data_rate, &phy_timing);
 	mtk_dsi_rxtx_control(mode_flags, lanes);
 	mtk_dsi_clk_hs_mode_disable();
-	mtk_dsi_config_vdo_timing(mode_flags, format, edid);
+	mtk_dsi_config_vdo_timing(mode_flags, format, lanes, edid, &phy_timing);
 	mtk_dsi_set_mode(mode_flags);
 	mtk_dsi_clk_hs_mode_enable();
 
diff --git a/src/soc/mediatek/common/include/soc/dsi_common.h b/src/soc/mediatek/common/include/soc/dsi_common.h
index bfdd4073d6..0738876aea 100644
--- a/src/soc/mediatek/common/include/soc/dsi_common.h
+++ b/src/soc/mediatek/common/include/soc/dsi_common.h
@@ -129,7 +129,9 @@ enum {
 	PACKED_PS_16BIT_RGB565 = (0 << 16),
 	LOOSELY_PS_18BIT_RGB666 = (1 << 16),
 	PACKED_PS_18BIT_RGB666 = (2 << 16),
-	PACKED_PS_24BIT_RGB888 = (3 << 16)
+	PACKED_PS_24BIT_RGB888 = (3 << 16),
+
+	DSI_PSCON_CUSTOM_HEADER_SHIFT = 26,
 };
 
 /* DSI_CMDQ_SIZE */
@@ -318,6 +320,8 @@ struct mtk_phy_timing {
 	u8 clk_hs_prepare;
 	u8 clk_hs_post;
 	u8 clk_hs_exit;
+
+	u32 d_phy;
 };
 
 /* Functions that each SOC should provide. */
diff --git a/src/soc/mediatek/mt8173/dsi.c b/src/soc/mediatek/mt8173/dsi.c
index 32f4f1ce1c..b6ff0bc51f 100644
--- a/src/soc/mediatek/mt8173/dsi.c
+++ b/src/soc/mediatek/mt8173/dsi.c
@@ -122,6 +122,8 @@ void mtk_dsi_override_phy_timing(struct mtk_phy_timing *timing)
 
 	timing->da_hs_sync = 0;
 	timing->clk_hs_exit = 2 * lpx;
+
+	timing->d_phy = 12;
 }
 
 void mtk_dsi_pin_drv_ctrl(void)
diff --git a/src/soc/mediatek/mt8173/include/soc/dsi.h b/src/soc/mediatek/mt8173/include/soc/dsi.h
index 99c51e62ff..43cbdcb909 100644
--- a/src/soc/mediatek/mt8173/include/soc/dsi.h
+++ b/src/soc/mediatek/mt8173/include/soc/dsi.h
@@ -22,6 +22,7 @@
 #define MTK_DSI_MIPI_RATIO_NUMERATOR 102
 #define MTK_DSI_MIPI_RATIO_DENOMINATOR 100
 #define MTK_DSI_DATA_RATE_MIN_MHZ 50
+#define PIXEL_STREAM_CUSTOM_HEADER 0
 
 /* MIPITX is SOC specific and cannot live in common. */