3 files changed, 49 insertions, 4 deletions
diff --git a/src/drivers/generic/bayhub/bh720.c b/src/drivers/generic/bayhub/bh720.c
index 24386642eb..40788cd31d 100644
--- a/src/drivers/generic/bayhub/bh720.c
+++ b/src/drivers/generic/bayhub/bh720.c
@@ -4,6 +4,7 @@
 
 #include <console/console.h>
 #include <device/device.h>
+#include <device/mmio.h>
 #include <device/path.h>
 #include <device/pci.h>
 #include <device/pci_ops.h>
@@ -11,8 +12,50 @@
 #include "chip.h"
 #include "bh720.h"
 
-__attribute__((weak)) void board_bh720(struct device *dev)
+static void bh720_program_hs200_mode(struct device *dev)
 {
+	u32 sdbar;
+	u32 bh720_pcr_data;
+
+	sdbar = pci_read_config32(dev, PCI_BASE_ADDRESS_1);
+
+	/* Enable Memory Access Function */
+	write32((void *)(sdbar + BH720_MEM_ACCESS_EN), 0x40000000);
+	write32((void *)(sdbar + BH720_MEM_RW_DATA), 0x80000000);
+	write32((void *)(sdbar + BH720_MEM_RW_ADR), 0x800000D0);
+
+	/* Set EMMC VCCQ 1.8V PCR 0x308[4] */
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_READ | BH720_PCR_EMMC_SETTING);
+	bh720_pcr_data = read32((void *)(sdbar + BH720_MEM_RW_DATA));
+	write32((void *)(sdbar + BH720_MEM_RW_DATA),
+		bh720_pcr_data | BH720_PCR_EMMC_SETTING_1_8V);
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_WRITE | BH720_PCR_EMMC_SETTING);
+
+	/* Set Base clock to 200MHz(PCR 0x304[31:16] = 0x2510) */
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_READ | BH720_PCR_DrvStrength_PLL);
+	bh720_pcr_data = read32((void *)(sdbar + BH720_MEM_RW_DATA));
+	bh720_pcr_data &= 0x0000FFFF;
+	bh720_pcr_data |= 0x2510 << 16;
+	write32((void *)(sdbar + BH720_MEM_RW_DATA), bh720_pcr_data);
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_WRITE | BH720_PCR_DrvStrength_PLL);
+
+	/* Use PLL Base clock PCR 0x3E4[22] = 1 */
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_READ | BH720_PCR_CSR);
+	bh720_pcr_data = read32((void *)(sdbar + BH720_MEM_RW_DATA));
+	write32((void *)(sdbar + BH720_MEM_RW_DATA),
+		bh720_pcr_data | BH720_PCR_CSR_EMMC_MODE_SEL);
+	write32((void *)(sdbar + BH720_MEM_RW_ADR),
+		BH720_MEM_RW_WRITE | BH720_PCR_CSR);
+
+	/* Disable Memory Access */
+	write32((void *)(sdbar + BH720_MEM_RW_DATA), 0x80000001);
+	write32((void *)(sdbar + BH720_MEM_RW_ADR), 0x800000D0);
+	write32((void *)(sdbar + BH720_MEM_ACCESS_EN), 0x80000000);
 }
 
 static void bh720_init(struct device *dev)
@@ -43,7 +86,8 @@ static void bh720_init(struct device *dev)
 		       pci_read_config32(dev, BH720_LINK_CTRL));
 	}
 
-	board_bh720(dev);
+	if (config && !config->disable_hs200_mode)
+		bh720_program_hs200_mode(dev);
 
 	if (config && config->vih_tuning_value) {
 		/* Tune VIH */
diff --git a/src/drivers/generic/bayhub/bh720.h b/src/drivers/generic/bayhub/bh720.h
index d8c3196495..cc5fbea07b 100644
--- a/src/drivers/generic/bayhub/bh720.h
+++ b/src/drivers/generic/bayhub/bh720.h
@@ -39,5 +39,3 @@ enum {
 	BH720_PCR_CSR			= 0x3e4,
 	BH720_PCR_CSR_EMMC_MODE_SEL	= BIT(22),
 };
-
-void board_bh720(struct device *dev);
diff --git a/src/drivers/generic/bayhub/chip.h b/src/drivers/generic/bayhub/chip.h
index f898378cb4..77b7512143 100644
--- a/src/drivers/generic/bayhub/chip.h
+++ b/src/drivers/generic/bayhub/chip.h
@@ -9,6 +9,9 @@ struct drivers_generic_bayhub_config {
 	/* 1 to enable power-saving mode, 0 to disable */
 	int power_saving;
 
+	/* When set, disables programming HS200 mode */
+	bool disable_hs200_mode;
+
 	/* CLK and DAT tuning values */
 	uint8_t vih_tuning_value;
 };