2 files changed, 64 insertions, 12 deletions

diff --git a/src/soc/rockchip/rk3399/include/soc/mipi.h b/src/soc/rockchip/rk3399/include/soc/mipi.h
index 077e05ef42..2ecd0ee19d 100644
--- a/src/soc/rockchip/rk3399/include/soc/mipi.h
+++ b/src/soc/rockchip/rk3399/include/soc/mipi.h
@@ -244,6 +244,17 @@ check_member(rk_mipi_regs, dsi_int_msk1, 0xc8);
 #define AFE_BIAS_BANDGAP_ANALOG_PROGRAMMABILITY 0x22
 #define HS_RX_CONTROL_OF_LANE_0 0x44
 
+#define GEN_CMD_EMPTY			BIT(0)
+#define GEN_CMD_FULL			BIT(1)
+#define GEN_PLD_W_EMPTY			BIT(2)
+#define GEN_PLD_W_FULL			BIT(3)
+#define GEN_PLD_R_EMPTY			BIT(4)
+#define GEN_PLD_R_FULL			BIT(5)
+#define GEN_RD_CMD_BUSY			BIT(6)
+
+#define MIPI_DSI_DCS_SHORT_WRITE	0x05
+#define MIPI_DSI_DCS_LONG_WRITE		0x39
+
 enum mipi_dsi_pixel_format {
 	MIPI_DSI_FMT_RGB888,
 	MIPI_DSI_FMT_RGB666,
diff --git a/src/soc/rockchip/rk3399/mipi.c b/src/soc/rockchip/rk3399/mipi.c
index 18d02c89d7..5375c4866b 100644
--- a/src/soc/rockchip/rk3399/mipi.c
+++ b/src/soc/rockchip/rk3399/mipi.c
@@ -460,22 +460,61 @@ static void rk_mipi_dsi_init(struct rk_mipi_dsi *dsi)
 		TX_ESC_CLK_DIVIDSION(esc_clk_division));
 }
 
-static int rk_mipi_dsi_dcs_transfer(struct rk_mipi_dsi *dsi, u32 hdr_val)
+static void rk_mipi_message_config(struct rk_mipi_dsi *dsi)
 {
-	int ret;
+	write32(&mipi_regs->dsi_lpclk_ctrl, 0);
+	write32(&mipi_regs->dsi_cmd_mode_cfg, CMD_MODE_ALL_LP);
+}
 
-	hdr_val = GEN_HDATA(hdr_val) | GEN_HTYPE(0x05);
-	ret = read32(&mipi_regs->dsi_cmd_pkt_status);
-	if (ret < 0) {
-		printk(BIOS_DEBUG, "failed to get available command FIFO\n");
-		return ret;
+static int rk_mipi_dsi_check_cmd_fifo(struct rk_mipi_dsi *dsi)
+{
+	struct stopwatch sw;
+	int val;
+
+	stopwatch_init_msecs_expire(&sw, 20);
+	do {
+		val = read32(&mipi_regs->dsi_cmd_pkt_status);
+		if (!(val & GEN_CMD_FULL))
+			return 0 ;
+	} while (!stopwatch_expired(&sw));
+
+	return -1;
+}
+
+static int rk_mipi_dsi_gen_pkt_hdr_write(struct rk_mipi_dsi *dsi, u32 hdr_val)
+{
+	int val;
+	struct stopwatch sw;
+	u32 mask;
+
+	if (rk_mipi_dsi_check_cmd_fifo(dsi)) {
+		printk(BIOS_ERR, "failed to get available command FIFO\n");
+		return -1;
 	}
 
-	write32(&mipi_regs->dsi_lpclk_ctrl, 0);
-	write32(&mipi_regs->dsi_cmd_mode_cfg, CMD_MODE_ALL_LP);
 	write32(&mipi_regs->dsi_gen_hdr, hdr_val);
 
-	return 0;
+	mask = GEN_CMD_EMPTY | GEN_PLD_W_EMPTY;
+	stopwatch_init_msecs_expire(&sw, 20);
+	do {
+		val = read32(&mipi_regs->dsi_cmd_pkt_status);
+		if ((val & mask) == mask)
+			return 0 ;
+	} while (!stopwatch_expired(&sw));
+	printk(BIOS_ERR, "failed to write command FIFO\n");
+
+	return -1;
+}
+
+static int rk_mipi_dsi_dcs_cmd(struct rk_mipi_dsi *dsi, u8 cmd)
+{
+	u32 val;
+
+	rk_mipi_message_config(dsi);
+
+	val = GEN_HDATA(cmd) | GEN_HTYPE(MIPI_DSI_DCS_SHORT_WRITE);
+
+	return rk_mipi_dsi_gen_pkt_hdr_write(dsi, val);
 }
 
 void rk_mipi_prepare(const struct edid *edid, u32 display_on_mdelay, u32 video_mode_mdelay)
@@ -501,10 +540,12 @@ void rk_mipi_prepare(const struct edid *edid, u32 display_on_mdelay, u32 video_m
 	rk_mipi_dsi_wait_for_two_frames(&rk_mipi, edid);
 
 	rk_mipi_dsi_set_mode(&rk_mipi, MIPI_DSI_CMD_MODE);
-	if (rk_mipi_dsi_dcs_transfer(&rk_mipi, MIPI_DCS_EXIT_SLEEP_MODE) < 0)
+
+	if (rk_mipi_dsi_dcs_cmd(&rk_mipi, MIPI_DCS_EXIT_SLEEP_MODE) < 0)
 		return;
 	mdelay(display_on_mdelay);
-	if (rk_mipi_dsi_dcs_transfer(&rk_mipi, MIPI_DCS_SET_DISPLAY_ON) < 0)
+
+	if (rk_mipi_dsi_dcs_cmd(&rk_mipi, MIPI_DCS_SET_DISPLAY_ON) < 0)
 		return;
 	mdelay(video_mode_mdelay);