4 files changed, 668 insertions, 88 deletions

diff --git a/arch/arm/mach-imx/imx8ulp/cgc.c b/arch/arm/mach-imx/imx8ulp/cgc.c
index 7bfc3862cd..38bcbb91e6 100644
--- a/arch/arm/mach-imx/imx8ulp/cgc.c
+++ b/arch/arm/mach-imx/imx8ulp/cgc.c
@@ -189,8 +189,8 @@ void cgc2_pll4_init(void)
 		;
 
 	/* Enable all 4 PFDs */
-	setbits_le32(&cgc2_regs->pll4pfdcfg, 30 << 0); /* 316.8Mhz for NIC_LPAV */
-	setbits_le32(&cgc2_regs->pll4pfdcfg, 18 << 8);
+	setbits_le32(&cgc2_regs->pll4pfdcfg, 18 << 0);
+	setbits_le32(&cgc2_regs->pll4pfdcfg, 30 << 8); /* 316.8Mhz for NIC_LPAV */
 	setbits_le32(&cgc2_regs->pll4pfdcfg, 12 << 16);
 	setbits_le32(&cgc2_regs->pll4pfdcfg, 24 << 24);
 
@@ -205,15 +205,144 @@ void cgc2_pll4_init(void)
 	clrbits_le32(&cgc2_regs->pll4div_pfd1, BIT(7) | BIT(15) | BIT(23) | BIT(31));
 }
 
+void cgc2_pll4_pfd_config(enum cgc_clk pllpfd, u32 pfd)
+{
+	void __iomem *reg = &cgc2_regs->pll4div_pfd0;
+	u32 halt_mask = BIT(7) | BIT(15);
+	u32 pfd_shift = (pllpfd - PLL4_PFD0) * 8;
+	u32 val;
+
+	if (pllpfd < PLL4_PFD0 || pllpfd > PLL4_PFD3)
+		return;
+
+	if ((pllpfd - PLL4_PFD0) >> 1)
+		reg = &cgc2_regs->pll4div_pfd1;
+
+	halt_mask = halt_mask << (((pllpfd - PLL4_PFD0) & 0x1) * 16);
+
+	/* halt pfd div */
+	setbits_le32(reg, halt_mask);
+
+	/* gate pfd */
+	setbits_le32(&cgc2_regs->pll4pfdcfg, BIT(7) << pfd_shift);
+
+	val = readl(&cgc2_regs->pll4pfdcfg);
+	val &= ~(0x3f << pfd_shift);
+	val |= (pfd << pfd_shift);
+	writel(val, &cgc2_regs->pll4pfdcfg);
+
+	/* ungate */
+	clrbits_le32(&cgc2_regs->pll4pfdcfg, BIT(7) << pfd_shift);
+
+	/* Wait stable */
+	while ((readl(&cgc2_regs->pll4pfdcfg) & (BIT(6) << pfd_shift))
+		!= (BIT(6) << pfd_shift))
+		;
+
+	/* enable pfd div */
+	clrbits_le32(reg, halt_mask);
+}
+
+void cgc2_pll4_pfddiv_config(enum cgc_clk pllpfddiv, u32 div)
+{
+	void __iomem *reg = &cgc2_regs->pll4div_pfd0;
+	u32 shift = ((pllpfddiv - PLL4_PFD0_DIV1) & 0x3) * 8;
+
+	if (pllpfddiv < PLL4_PFD0_DIV1 || pllpfddiv > PLL4_PFD3_DIV2)
+		return;
+
+	if ((pllpfddiv - PLL4_PFD0_DIV1) >> 2)
+		reg = &cgc2_regs->pll4div_pfd1;
+
+	/* Halt pfd div */
+	setbits_le32(reg, BIT(7) << shift);
+
+	/* Clear div */
+	clrbits_le32(reg, 0x3f << shift);
+
+	/* Set div*/
+	setbits_le32(reg, div << shift);
+
+	/* Enable pfd div */
+	clrbits_le32(reg, BIT(7) << shift);
+}
+
 void cgc2_ddrclk_config(u32 src, u32 div)
 {
+	/* If reg lock is set, wait until unlock by HW */
+	/* This lock is triggered by div updating and ddrclk halt status change, */
+	while ((readl(&cgc2_regs->ddrclk) & BIT(31)))
+		;
+
 	writel((src << 28) | (div << 21), &cgc2_regs->ddrclk);
 	/* wait for DDRCLK switching done */
 	while (!(readl(&cgc2_regs->ddrclk) & BIT(27)))
 		;
 }
 
-u32 decode_pll(enum cgc1_clk pll)
+void cgc2_ddrclk_wait_unlock(void)
+{
+	while ((readl(&cgc2_regs->ddrclk) & BIT(31)))
+		;
+}
+
+void cgc2_lpav_init(enum cgc_clk clk)
+{
+	u32 i, scs, reg;
+	const enum cgc_clk src[] = {FRO, PLL4_PFD1, SOSC, LVDS};
+
+	reg = readl(&cgc2_regs->niclpavclk);
+	scs = (reg >> 28) & 0x3;
+
+	for (i = 0; i < 4; i++) {
+		if (clk == src[i]) {
+			if (scs == i)
+				return;
+
+			reg &= ~(0x3 << 28);
+			reg |= (i << 28);
+
+			writel(reg, &cgc2_regs->niclpavclk);
+			break;
+		}
+	}
+
+	if (i == 4)
+		printf("Invalid clock source [%u] for LPAV\n", clk);
+}
+
+u32 cgc2_nic_get_rate(enum cgc_clk clk)
+{
+	u32 reg, rate;
+	u32 scs, lpav_axi_clk, lpav_ahb_clk, lpav_bus_clk;
+	const enum cgc_clk src[] = {FRO, PLL4_PFD1, SOSC, LVDS};
+
+	reg = readl(&cgc2_regs->niclpavclk);
+	scs = (reg >> 28) & 0x3;
+	lpav_axi_clk = ((reg >> 21) & 0x3f) + 1;
+	lpav_ahb_clk = ((reg >> 14) & 0x3f) + 1;
+	lpav_bus_clk = ((reg >> 7) & 0x3f) + 1;
+
+	rate = cgc_clk_get_rate(src[scs]);
+
+	switch (clk) {
+	case LPAV_AXICLK:
+		rate = rate / lpav_axi_clk;
+		break;
+	case LPAV_AHBCLK:
+		rate = rate / (lpav_axi_clk * lpav_ahb_clk);
+		break;
+	case LPAV_BUSCLK:
+		rate = rate / (lpav_axi_clk * lpav_bus_clk);
+		break;
+	default:
+		return 0;
+	}
+
+	return rate;
+}
+
+u32 decode_pll(enum cgc_clk pll)
 {
 	u32 reg, infreq, mult;
 	u32 num, denom;
@@ -247,6 +376,17 @@ u32 decode_pll(enum cgc1_clk pll)
 		num = readl(&cgc1_regs->pll3num) & 0x3FFFFFFF;
 
 		return (u64)infreq * mult + (u64)infreq * num / denom;
+	case PLL4:
+		reg = readl(&cgc2_regs->pll4csr);
+		if (!(reg & BIT(24)))
+			return 0;
+
+		reg = readl(&cgc2_regs->pll4cfg);
+		mult = (reg >> 16) & 0x7F;
+		denom = readl(&cgc2_regs->pll4denom) & 0x3FFFFFFF;
+		num = readl(&cgc2_regs->pll4num) & 0x3FFFFFFF;
+
+		return (u64)infreq * mult + (u64)infreq * num / denom;
 	default:
 		printf("Unsupported pll clocks %d\n", pll);
 		break;
@@ -255,93 +395,117 @@ u32 decode_pll(enum cgc1_clk pll)
 	return 0;
 }
 
-u32 cgc1_pll3_vcodiv_rate(void)
+u32 cgc_pll_vcodiv_rate(enum cgc_clk clk)
 {
 	u32 reg, gate, div;
+	void __iomem *plldiv_vco;
+	enum cgc_clk pll;
+
+	if (clk == PLL3_VCODIV) {
+		plldiv_vco = &cgc1_regs->pll3div_vco;
+		pll = PLL3;
+	} else {
+		plldiv_vco = &cgc2_regs->pll4div_vco;
+		pll = PLL4;
+	}
 
-	reg = readl(&cgc1_regs->pll3div_vco);
+	reg = readl(plldiv_vco);
 	gate = BIT(7) & reg;
 	div = reg & 0x3F;
 
-	return gate ? 0 : decode_pll(PLL3) / (div + 1);
+	return gate ? 0 : decode_pll(pll) / (div + 1);
 }
 
-u32 cgc1_pll3_pfd_rate(enum cgc1_clk clk)
+u32 cgc_pll_pfd_rate(enum cgc_clk clk)
 {
 	u32 index, gate, vld, reg;
+	void __iomem *pllpfdcfg;
+	enum cgc_clk pll;
 
 	switch (clk) {
 	case PLL3_PFD0:
-		index = 0;
-		break;
 	case PLL3_PFD1:
-		index = 1;
-		break;
 	case PLL3_PFD2:
-		index = 2;
-		break;
 	case PLL3_PFD3:
-		index = 3;
+		index = clk - PLL3_PFD0;
+		pllpfdcfg = &cgc1_regs->pll3pfdcfg;
+		pll = PLL3;
+		break;
+	case PLL4_PFD0:
+	case PLL4_PFD1:
+	case PLL4_PFD2:
+	case PLL4_PFD3:
+		index = clk - PLL4_PFD0;
+		pllpfdcfg = &cgc2_regs->pll4pfdcfg;
+		pll = PLL4;
 		break;
 	default:
 		return 0;
 	}
 
-	reg = readl(&cgc1_regs->pll3pfdcfg);
+	reg = readl(pllpfdcfg);
 	gate = reg & (BIT(7) << (index * 8));
 	vld = reg & (BIT(6) << (index * 8));
 
 	if (gate || !vld)
 		return 0;
 
-	return (u64)decode_pll(PLL3) * 18 / ((reg >> (index * 8)) & 0x3F);
+	return (u64)decode_pll(pll) * 18 / ((reg >> (index * 8)) & 0x3F);
 }
 
-u32 cgc1_pll3_pfd_div(enum cgc1_clk clk)
+u32 cgc_pll_pfd_div(enum cgc_clk clk)
 {
 	void __iomem *base;
 	u32 pfd, index, gate, reg;
 
 	switch (clk) {
 	case PLL3_PFD0_DIV1:
-		base = &cgc1_regs->pll3div_pfd0;
-		pfd = PLL3_PFD0;
-		index = 0;
-		break;
 	case PLL3_PFD0_DIV2:
 		base = &cgc1_regs->pll3div_pfd0;
 		pfd = PLL3_PFD0;
-		index = 1;
+		index = clk - PLL3_PFD0_DIV1;
 		break;
 	case PLL3_PFD1_DIV1:
-		base = &cgc1_regs->pll3div_pfd0;
-		pfd = PLL3_PFD1;
-		index = 2;
-		break;
 	case PLL3_PFD1_DIV2:
 		base = &cgc1_regs->pll3div_pfd0;
 		pfd = PLL3_PFD1;
-		index = 3;
+		index = clk - PLL3_PFD0_DIV1;
 		break;
 	case PLL3_PFD2_DIV1:
-		base = &cgc1_regs->pll3div_pfd1;
-		pfd = PLL3_PFD2;
-		index = 0;
-		break;
 	case PLL3_PFD2_DIV2:
 		base = &cgc1_regs->pll3div_pfd1;
 		pfd = PLL3_PFD2;
-		index = 1;
+		index = clk - PLL3_PFD2_DIV1;
 		break;
 	case PLL3_PFD3_DIV1:
-		base = &cgc1_regs->pll3div_pfd1;
-		pfd = PLL3_PFD3;
-		index = 2;
-		break;
 	case PLL3_PFD3_DIV2:
 		base = &cgc1_regs->pll3div_pfd1;
 		pfd = PLL3_PFD3;
-		index = 3;
+		index = clk - PLL3_PFD2_DIV1;
+		break;
+	case PLL4_PFD0_DIV1:
+	case PLL4_PFD0_DIV2:
+		base = &cgc2_regs->pll4div_pfd0;
+		pfd = PLL4_PFD0;
+		index = clk - PLL4_PFD0_DIV1;
+		break;
+	case PLL4_PFD1_DIV1:
+	case PLL4_PFD1_DIV2:
+		base = &cgc2_regs->pll4div_pfd0;
+		pfd = PLL4_PFD1;
+		index = clk - PLL4_PFD0_DIV1;
+		break;
+	case PLL4_PFD2_DIV1:
+	case PLL4_PFD2_DIV2:
+		base = &cgc2_regs->pll4div_pfd1;
+		pfd = PLL4_PFD2;
+		index = clk - PLL4_PFD2_DIV1;
+		break;
+	case PLL4_PFD3_DIV1:
+	case PLL4_PFD3_DIV2:
+		base = &cgc2_regs->pll4div_pfd1;
+		pfd = PLL4_PFD3;
+		index = clk - PLL4_PFD2_DIV1;
 		break;
 	default:
 		return 0;
@@ -353,10 +517,52 @@ u32 cgc1_pll3_pfd_div(enum cgc1_clk clk)
 	if (gate)
 		return 0;
 
-	return cgc1_pll3_pfd_rate(pfd) / (((reg >> (index * 8)) & 0x3F) + 1);
+	return cgc_pll_pfd_rate(pfd) / (((reg >> (index * 8)) & 0x3F) + 1);
+}
+
+u32 cgc1_nic_get_rate(enum cgc_clk clk)
+{
+	u32 reg, rate;
+	u32 scs, nic_ad_divplat, nic_per_divplat;
+	u32 xbar_ad_divplat, xbar_divbus, ad_slow;
+	const enum cgc_clk src[] = {FRO, PLL3_PFD0, SOSC, LVDS};
+
+	reg = readl(&cgc1_regs->nicclk);
+	scs = (reg >> 28) & 0x3;
+	nic_ad_divplat = ((reg >> 21) & 0x3f) + 1;
+	nic_per_divplat = ((reg >> 14) & 0x3f) + 1;
+
+	reg = readl(&cgc1_regs->xbarclk);
+	xbar_ad_divplat = ((reg >> 14) & 0x3f) + 1;
+	xbar_divbus = ((reg >> 7) & 0x3f) + 1;
+	ad_slow = (reg & 0x3f) + 1;
+
+	rate = cgc_clk_get_rate(src[scs]);
+
+	switch (clk) {
+	case NIC_APCLK:
+		rate = rate / nic_ad_divplat;
+		break;
+	case NIC_PERCLK:
+		rate = rate / (nic_ad_divplat * nic_per_divplat);
+		break;
+	case XBAR_APCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat);
+		break;
+	case XBAR_BUSCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat * xbar_divbus);
+		break;
+	case AD_SLOWCLK:
+		rate = rate / (nic_ad_divplat * xbar_ad_divplat * ad_slow);
+		break;
+	default:
+		return 0;
+	}
+
+	return rate;
 }
 
-u32 cgc1_sosc_div(enum cgc1_clk clk)
+u32 cgc1_sosc_div(enum cgc_clk clk)
 {
 	u32 reg, gate, index;
 
@@ -385,7 +591,7 @@ u32 cgc1_sosc_div(enum cgc1_clk clk)
 	return 24000000 / (((reg >> (index * 8)) & 0x3F) + 1);
 }
 
-u32 cgc1_fro_div(enum cgc1_clk clk)
+u32 cgc1_fro_div(enum cgc_clk clk)
 {
 	u32 reg, gate, vld, index;
 
@@ -415,9 +621,11 @@ u32 cgc1_fro_div(enum cgc1_clk clk)
 	return 24000000 / (((reg >> (index * 8)) & 0x3F) + 1);
 }
 
-u32 cgc1_clk_get_rate(enum cgc1_clk clk)
+u32 cgc_clk_get_rate(enum cgc_clk clk)
 {
 	switch (clk) {
+	case LVDS:
+		return 0; /* No external LVDS clock used */
 	case SOSC:
 	case SOSC_DIV1:
 	case SOSC_DIV2:
@@ -429,16 +637,21 @@ u32 cgc1_clk_get_rate(enum cgc1_clk clk)
 	case FRO_DIV3:
 		return cgc1_fro_div(clk);
 	case PLL2:
-		return decode_pll(PLL2);
 	case PLL3:
-		return decode_pll(PLL3);
+	case PLL4:
+		return decode_pll(clk);
 	case PLL3_VCODIV:
-		return cgc1_pll3_vcodiv_rate();
+	case PLL4_VCODIV:
+		return cgc_pll_vcodiv_rate(clk);
 	case PLL3_PFD0:
 	case PLL3_PFD1:
 	case PLL3_PFD2:
 	case PLL3_PFD3:
-		return cgc1_pll3_pfd_rate(clk);
+	case PLL4_PFD0:
+	case PLL4_PFD1:
+	case PLL4_PFD2:
+	case PLL4_PFD3:
+		return cgc_pll_pfd_rate(clk);
 	case PLL3_PFD0_DIV1:
 	case PLL3_PFD0_DIV2:
 	case PLL3_PFD1_DIV1:
@@ -447,9 +660,27 @@ u32 cgc1_clk_get_rate(enum cgc1_clk clk)
 	case PLL3_PFD2_DIV2:
 	case PLL3_PFD3_DIV1:
 	case PLL3_PFD3_DIV2:
-		return cgc1_pll3_pfd_div(clk);
+	case PLL4_PFD0_DIV1:
+	case PLL4_PFD0_DIV2:
+	case PLL4_PFD1_DIV1:
+	case PLL4_PFD1_DIV2:
+	case PLL4_PFD2_DIV1:
+	case PLL4_PFD2_DIV2:
+	case PLL4_PFD3_DIV1:
+	case PLL4_PFD3_DIV2:
+		return cgc_pll_pfd_div(clk);
+	case NIC_APCLK:
+	case NIC_PERCLK:
+	case XBAR_APCLK:
+	case XBAR_BUSCLK:
+	case AD_SLOWCLK:
+		return cgc1_nic_get_rate(clk);
+	case LPAV_AXICLK:
+	case LPAV_AHBCLK:
+	case LPAV_BUSCLK:
+		return cgc2_nic_get_rate(clk);
 	default:
-		printf("Unsupported cgc1 clock: %d\n", clk);
+		printf("Unsupported cgc clock: %d\n", clk);
 		return 0;
 	}
 }
diff --git a/arch/arm/mach-imx/imx8ulp/clock.c b/arch/arm/mach-imx/imx8ulp/clock.c
index ebbaad4106..91580b2c29 100644
--- a/arch/arm/mach-imx/imx8ulp/clock.c
+++ b/arch/arm/mach-imx/imx8ulp/clock.c
@@ -27,7 +27,7 @@ DECLARE_GLOBAL_DATA_PTR;
 #define PLL_USB_LOCK_MASK		(0x01 << 31)
 #define PCC5_LPDDR4_ADDR 0x2da70108
 
-static void lpuart_set_clk(u32 index, enum cgc1_clk clk)
+static void lpuart_set_clk(u32 index, enum cgc_clk clk)
 {
 	const u32 lpuart_pcc_slots[] = {
 		LPUART4_PCC3_SLOT,
@@ -97,6 +97,9 @@ void ddrphy_pll_lock(void)
 
 void init_clk_ddr(void)
 {
+	/* disable the ddr pcc */
+	writel(0xc0000000, PCC5_LPDDR4_ADDR);
+
 	/* enable pll4 and ddrclk*/
 	cgc2_pll4_init();
 	cgc2_ddrclk_config(1, 1);
@@ -104,6 +107,9 @@ void init_clk_ddr(void)
 	/* enable ddr pcc */
 	writel(0xd0000000, PCC5_LPDDR4_ADDR);
 
+	/* Wait until ddrclk reg lock bit is cleared, so that the div update is finished */
+	cgc2_ddrclk_wait_unlock();
+
 	/* for debug */
 	/* setclkout_ddr(); */
 }
@@ -141,6 +147,9 @@ int set_ddr_clk(u32 phy_freq_mhz)
 		return -EINVAL;
 	}
 
+	/* Wait until ddrclk reg lock bit is cleared, so that the div update is finished */
+	cgc2_ddrclk_wait_unlock();
+
 	return 0;
 }
 
@@ -186,6 +195,9 @@ int enable_i2c_clk(unsigned char enable, u32 i2c_num)
 		LPI2C7_PCC4_SLOT << 8 | 4,
 	};
 
+	if (i2c_num == 0)
+		return 0;
+
 	if (i2c_num < 4 || i2c_num > 7)
 		return -EINVAL;
 
@@ -214,6 +226,9 @@ u32 imx_get_i2cclk(u32 i2c_num)
 		LPI2C7_PCC4_SLOT << 8 | 4,
 	};
 
+	if (i2c_num == 0)
+		return 24000000;
+
 	if (i2c_num < 4 || i2c_num > 7)
 		return 0;
 
@@ -317,6 +332,99 @@ int enable_usb_pll(ulong usb_phy_base)
 	return 0;
 }
 
+void enable_mipi_dsi_clk(unsigned char enable)
+{
+	if (enable) {
+		pcc_clock_enable(5, DSI_PCC5_SLOT, false);
+		pcc_reset_peripheral(5, DSI_PCC5_SLOT, true);
+		pcc_clock_sel(5, DSI_PCC5_SLOT, PLL4_PFD3_DIV2);
+		pcc_clock_div_config(5, DSI_PCC5_SLOT, 0, 6);
+		pcc_clock_enable(5, DSI_PCC5_SLOT, true);
+		pcc_reset_peripheral(5, DSI_PCC5_SLOT, false);
+	} else {
+		pcc_clock_enable(5, DSI_PCC5_SLOT, false);
+		pcc_reset_peripheral(5, DSI_PCC5_SLOT, true);
+	}
+}
+
+void enable_adc1_clk(bool enable)
+{
+	if (enable) {
+		pcc_clock_enable(1, ADC1_PCC1_SLOT, false);
+		pcc_clock_sel(1, ADC1_PCC1_SLOT, CM33_BUSCLK);
+		pcc_clock_enable(1, ADC1_PCC1_SLOT, true);
+		pcc_reset_peripheral(1, ADC1_PCC1_SLOT, false);
+	} else {
+		pcc_clock_enable(1, ADC1_PCC1_SLOT, false);
+	}
+}
+
+void reset_lcdclk(void)
+{
+	/* Disable clock and reset dcnano*/
+	pcc_clock_enable(5, DCNANO_PCC5_SLOT, false);
+	pcc_reset_peripheral(5, DCNANO_PCC5_SLOT, true);
+}
+
+void mxs_set_lcdclk(u32 base_addr, u32 freq_in_khz)
+{
+	u8 pcd, best_pcd = 0;
+	u32 frac, rate, parent_rate, pfd, div;
+	u32 best_pfd = 0, best_frac = 0, best = 0, best_div = 0;
+	u32 pll4_rate;
+
+	pcc_clock_enable(5, DCNANO_PCC5_SLOT, false);
+
+	pll4_rate = cgc_clk_get_rate(PLL4);
+	pll4_rate = pll4_rate / 1000;  /* Change to khz*/
+
+	debug("PLL4 rate %ukhz\n", pll4_rate);
+
+	for (pfd = 12; pfd <= 35; pfd++) {
+		parent_rate = pll4_rate;
+		parent_rate = parent_rate * 18 / pfd;
+
+		for (div = 1; div <= 64; div++) {
+			parent_rate = parent_rate / div;
+
+			for (pcd = 0; pcd < 8; pcd++) {
+				for (frac = 0; frac < 2; frac++) {
+					if (pcd == 0 && frac == 1)
+						continue;
+
+					rate = parent_rate * (frac + 1) / (pcd + 1);
+					if (rate > freq_in_khz)
+						continue;
+
+					if (best == 0 || rate > best) {
+						best = rate;
+						best_pfd = pfd;
+						best_frac = frac;
+						best_pcd = pcd;
+						best_div = div;
+					}
+				}
+			}
+		}
+	}
+
+	if (best == 0) {
+		printf("Can't find parent clock for LCDIF, target freq: %u\n", freq_in_khz);
+		return;
+	}
+
+	debug("LCD target rate %ukhz, best rate %ukhz, frac %u, pcd %u, best_pfd %u, best_div %u\n",
+	      freq_in_khz, best, best_frac, best_pcd, best_pfd, best_div);
+
+	cgc2_pll4_pfd_config(PLL4_PFD0, best_pfd);
+	cgc2_pll4_pfddiv_config(PLL4_PFD0_DIV1, best_div - 1);
+
+	pcc_clock_sel(5, DCNANO_PCC5_SLOT, PLL4_PFD0_DIV1);
+	pcc_clock_div_config(5, DCNANO_PCC5_SLOT, best_frac, best_pcd + 1);
+	pcc_clock_enable(5, DCNANO_PCC5_SLOT, true);
+	pcc_reset_peripheral(5, DCNANO_PCC5_SLOT, false);
+}
+
 u32 mxc_get_clock(enum mxc_clock clk)
 {
 	switch (clk) {
@@ -327,7 +435,7 @@ u32 mxc_get_clock(enum mxc_clock clk)
 	case MXC_ESDHC3_CLK:
 		return pcc_clock_get_rate(4, SDHC2_PCC4_SLOT);
 	case MXC_ARM_CLK:
-		return cgc1_clk_get_rate(PLL2);
+		return cgc_clk_get_rate(PLL2);
 	default:
 		return 0;
 	}
@@ -376,16 +484,40 @@ int do_mx8ulp_showclocks(struct cmd_tbl *cmdtp, int flag, int argc, char * const
 	printf("SDHC1 %8d MHz\n", pcc_clock_get_rate(4, SDHC1_PCC4_SLOT) / 1000000);
 	printf("SDHC2 %8d MHz\n", pcc_clock_get_rate(4, SDHC2_PCC4_SLOT) / 1000000);
 
-	printf("SOSC %8d MHz\n", cgc1_clk_get_rate(SOSC) / 1000000);
-	printf("FRO %8d MHz\n", cgc1_clk_get_rate(FRO) / 1000000);
-	printf("PLL2 %8d MHz\n", cgc1_clk_get_rate(PLL2) / 1000000);
-	printf("PLL3 %8d MHz\n", cgc1_clk_get_rate(PLL3) / 1000000);
-	printf("PLL3_VCODIV %8d MHz\n", cgc1_clk_get_rate(PLL3_VCODIV) / 1000000);
-	printf("PLL3_PFD0 %8d MHz\n", cgc1_clk_get_rate(PLL3_PFD0) / 1000000);
-	printf("PLL3_PFD1 %8d MHz\n", cgc1_clk_get_rate(PLL3_PFD1) / 1000000);
-	printf("PLL3_PFD2 %8d MHz\n", cgc1_clk_get_rate(PLL3_PFD2) / 1000000);
-	printf("PLL3_PFD3 %8d MHz\n", cgc1_clk_get_rate(PLL3_PFD3) / 1000000);
-
+	printf("SOSC %8d MHz\n", cgc_clk_get_rate(SOSC) / 1000000);
+	printf("FRO %8d MHz\n", cgc_clk_get_rate(FRO) / 1000000);
+	printf("PLL2 %8d MHz\n", cgc_clk_get_rate(PLL2) / 1000000);
+	printf("PLL3 %8d MHz\n", cgc_clk_get_rate(PLL3) / 1000000);
+	printf("PLL3_VCODIV %8d MHz\n", cgc_clk_get_rate(PLL3_VCODIV) / 1000000);
+	printf("PLL3_PFD0 %8d MHz\n", cgc_clk_get_rate(PLL3_PFD0) / 1000000);
+	printf("PLL3_PFD1 %8d MHz\n", cgc_clk_get_rate(PLL3_PFD1) / 1000000);
+	printf("PLL3_PFD2 %8d MHz\n", cgc_clk_get_rate(PLL3_PFD2) / 1000000);
+	printf("PLL3_PFD3 %8d MHz\n", cgc_clk_get_rate(PLL3_PFD3) / 1000000);
+
+	printf("PLL4_PFD0 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD0) / 1000000);
+	printf("PLL4_PFD1 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD1) / 1000000);
+	printf("PLL4_PFD2 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD2) / 1000000);
+	printf("PLL4_PFD3 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD3) / 1000000);
+
+	printf("PLL4_PFD0_DIV1 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD0_DIV1) / 1000000);
+	printf("PLL4_PFD0_DIV2 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD0_DIV2) / 1000000);
+	printf("PLL4_PFD1_DIV1 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD1_DIV1) / 1000000);
+	printf("PLL4_PFD1_DIV2 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD1_DIV2) / 1000000);
+
+	printf("PLL4_PFD2_DIV1 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD2_DIV1) / 1000000);
+	printf("PLL4_PFD2_DIV2 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD2_DIV2) / 1000000);
+	printf("PLL4_PFD3_DIV1 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD3_DIV1) / 1000000);
+	printf("PLL4_PFD3_DIV2 %8d MHz\n", cgc_clk_get_rate(PLL4_PFD3_DIV2) / 1000000);
+
+	printf("LPAV_AXICLK %8d MHz\n", cgc_clk_get_rate(LPAV_AXICLK) / 1000000);
+	printf("LPAV_AHBCLK %8d MHz\n", cgc_clk_get_rate(LPAV_AHBCLK) / 1000000);
+	printf("LPAV_BUSCLK %8d MHz\n", cgc_clk_get_rate(LPAV_BUSCLK) / 1000000);
+	printf("NIC_APCLK %8d MHz\n", cgc_clk_get_rate(NIC_APCLK) / 1000000);
+
+	printf("NIC_PERCLK %8d MHz\n", cgc_clk_get_rate(NIC_PERCLK) / 1000000);
+	printf("XBAR_APCLK %8d MHz\n", cgc_clk_get_rate(XBAR_APCLK) / 1000000);
+	printf("XBAR_BUSCLK %8d MHz\n", cgc_clk_get_rate(XBAR_BUSCLK) / 1000000);
+	printf("AD_SLOWCLK %8d MHz\n", cgc_clk_get_rate(AD_SLOWCLK) / 1000000);
 	return 0;
 }
 
diff --git a/arch/arm/mach-imx/imx8ulp/pcc.c b/arch/arm/mach-imx/imx8ulp/pcc.c
index 711b685cd7..7909d770af 100644
--- a/arch/arm/mach-imx/imx8ulp/pcc.c
+++ b/arch/arm/mach-imx/imx8ulp/pcc.c
@@ -12,10 +12,25 @@
 #include <asm/arch/cgc.h>
 #include <asm/arch/sys_proto.h>
 
-#define cgc1_clk_TYPES 2
-#define cgc1_clk_NUM 8
+#define cgc_clk_TYPES 2
+#define cgc_clk_NUM 8
 
-static enum cgc1_clk pcc3_clksrc[][8] = {
+static enum cgc_clk pcc1_clksrc[][8] = {
+	{
+	},
+	{
+		DUMMY0_CLK,
+		LPOSC,
+		SOSC_DIV2,
+		FRO_DIV2,
+		CM33_BUSCLK,
+		PLL1_VCO_DIV,
+		PLL0_PFD2_DIV,
+		PLL0_PFD1_DIV,
+	}
+};
+
+static enum cgc_clk pcc3_clksrc[][8] = {
 	{
 	},
 	{	DUMMY0_CLK,
@@ -29,7 +44,7 @@ static enum cgc1_clk pcc3_clksrc[][8] = {
 	}
 };
 
-static enum cgc1_clk pcc4_clksrc[][8] = {
+static enum cgc_clk pcc4_clksrc[][8] = {
 	{
 		DUMMY0_CLK,
 		SOSC_DIV1,
@@ -52,6 +67,34 @@ static enum cgc1_clk pcc4_clksrc[][8] = {
 	}
 };
 
+static enum cgc_clk pcc5_clksrc[][8] = {
+	{
+		DUMMY0_CLK,
+		PLL4_PFD3_DIV2,
+		PLL4_PFD2_DIV2,
+		PLL4_PFD2_DIV1,
+		PLL4_PFD1_DIV2,
+		PLL4_PFD1_DIV1,
+		PLL4_PFD0_DIV2,
+		PLL4_PFD0_DIV1
+	},
+	{
+		DUMMY0_CLK,
+		DUMMY1_CLK,
+		LPOSC,
+		SOSC_DIV2,
+		FRO_DIV2,
+		LPAV_BUSCLK,
+		PLL4_VCODIV,
+		PLL4_PFD3_DIV1
+	}
+};
+
+static struct pcc_entry pcc1_arrays[] = {
+	{PCC1_RBASE, ADC1_PCC1_SLOT, CLKSRC_PER_BUS, PCC_NO_DIV, PCC_HAS_RST_B},
+	{}
+};
+
 static struct pcc_entry pcc3_arrays[] = {
 	{PCC3_RBASE, DMA1_MP_PCC3_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B},
 	{PCC3_RBASE, DMA1_CH0_PCC3_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B},
@@ -136,12 +179,79 @@ static struct pcc_entry pcc4_arrays[] = {
 	{}
 };
 
+static struct pcc_entry pcc5_arrays[] = {
+	{PCC5_RBASE, DMA2_MP_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH0_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH1_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH2_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH3_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH4_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH5_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH6_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH7_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH8_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH9_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH10_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH11_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH12_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH13_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH14_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH15_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH16_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH17_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH18_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH19_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH20_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH21_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH22_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH23_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH24_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH25_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH26_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH27_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH28_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH29_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH30_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, DMA2_CH31_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, MU2_B_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, MU3_B_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, SEMA42_2_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, CMC2_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, AVD_SIM_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, LPAV_CGC_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, PCC5_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, TPM8_PCC5_SLOT,		CLKSRC_PER_BUS, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, SAI6_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, SAI7_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, SPDIF_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, ISI_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, CSI_REGS_PCC5_SLOT,	CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, CSI_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, DSI_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, WDOG5_PCC5_SLOT,		CLKSRC_PER_BUS, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, EPDC_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, PXP_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, SFA2_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, GPU2D_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, GPU3D_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, DCNANO_PCC5_SLOT,		CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, LPDDR4_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_HAS_RST_B },
+	{PCC5_RBASE, CSI_CLK_UI_PCC5_SLOT,	CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, CSI_CLK_ESC_PCC5_SLOT,	CLKSRC_PER_PLAT, PCC_HAS_DIV, PCC_NO_RST_B },
+	{PCC5_RBASE, RGPIOD_PCC5_SLOT,		CLKSRC_NO_PCS, PCC_NO_DIV, PCC_NO_RST_B },
+	{}
+};
+
 static int find_pcc_entry(int pcc_controller, int pcc_clk_slot, struct pcc_entry **out)
 {
 	struct pcc_entry *pcc_array;
 	int index = 0;
 
 	switch (pcc_controller) {
+	case 1:
+		pcc_array = pcc1_arrays;
+		*out = &pcc1_arrays[0];
+		break;
 	case 3:
 		pcc_array = pcc3_arrays;
 		*out = &pcc3_arrays[0];
@@ -150,6 +260,10 @@ static int find_pcc_entry(int pcc_controller, int pcc_clk_slot, struct pcc_entry
 		pcc_array = pcc4_arrays;
 		*out = &pcc4_arrays[0];
 		break;
+	case 5:
+		pcc_array = pcc5_arrays;
+		*out = &pcc5_arrays[0];
+		break;
 	default:
 		printf("Not supported pcc_controller: %d\n", pcc_controller);
 		return -EINVAL;
@@ -199,12 +313,12 @@ int pcc_clock_enable(int pcc_controller, int pcc_clk_slot, bool enable)
 }
 
 /* The clock source select needs clock is disabled */
-int pcc_clock_sel(int pcc_controller, int pcc_clk_slot, enum cgc1_clk src)
+int pcc_clock_sel(int pcc_controller, int pcc_clk_slot, enum cgc_clk src)
 {
 	u32 val, i, clksrc_type;
 	void __iomem *reg;
 	struct pcc_entry *pcc_array;
-	enum cgc1_clk *cgc1_clk_array;
+	enum cgc_clk *cgc_clk_array;
 	int clk;
 
 	clk = find_pcc_entry(pcc_controller, pcc_clk_slot, &pcc_array);
@@ -220,19 +334,23 @@ int pcc_clock_sel(int pcc_controller, int pcc_clk_slot, enum cgc1_clk src)
 		return -EPERM;
 	}
 
-	if (pcc_controller == 3)
-		cgc1_clk_array = pcc3_clksrc[clksrc_type];
+	if (pcc_controller == 1)
+		cgc_clk_array = pcc1_clksrc[clksrc_type];
+	else if (pcc_controller == 3)
+		cgc_clk_array = pcc3_clksrc[clksrc_type];
+	else if (pcc_controller == 4)
+		cgc_clk_array = pcc4_clksrc[clksrc_type];
 	else
-		cgc1_clk_array = pcc4_clksrc[clksrc_type];
+		cgc_clk_array = pcc5_clksrc[clksrc_type];
 
-	for (i = 0; i < cgc1_clk_NUM; i++) {
-		if (cgc1_clk_array[i] == src) {
+	for (i = 0; i < cgc_clk_NUM; i++) {
+		if (cgc_clk_array[i] == src) {
 			/* Find the clock src, then set it to PCS */
 			break;
 		}
 	}
 
-	if (i == cgc1_clk_NUM) {
+	if (i == cgc_clk_NUM) {
 		printf("No parent in PCS of PCC %d, invalid scg_clk %d\n", clk, src);
 		return -EINVAL;
 	}
@@ -320,13 +438,13 @@ bool pcc_clock_is_enable(int pcc_controller, int pcc_clk_slot)
 	return false;
 }
 
-int pcc_clock_get_clksrc(int pcc_controller, int pcc_clk_slot, enum cgc1_clk *src)
+int pcc_clock_get_clksrc(int pcc_controller, int pcc_clk_slot, enum cgc_clk *src)
 {
 	u32 val, clksrc_type;
 	void __iomem *reg;
 	struct pcc_entry *pcc_array;
 	int clk;
-	enum cgc1_clk *cgc1_clk_array;
+	enum cgc_clk *cgc_clk_array;
 
 	clk = find_pcc_entry(pcc_controller, pcc_clk_slot, &pcc_array);
 	if (clk < 0)
@@ -360,11 +478,13 @@ int pcc_clock_get_clksrc(int pcc_controller, int pcc_clk_slot, enum cgc1_clk *sr
 	}
 
 	if (pcc_controller == 3)
-		cgc1_clk_array = pcc3_clksrc[clksrc_type];
+		cgc_clk_array = pcc3_clksrc[clksrc_type];
+	else if (pcc_controller == 4)
+		cgc_clk_array = pcc4_clksrc[clksrc_type];
 	else
-		cgc1_clk_array = pcc4_clksrc[clksrc_type];
+		cgc_clk_array = pcc5_clksrc[clksrc_type];
 
-	*src = cgc1_clk_array[val];
+	*src = cgc_clk_array[val];
 
 	debug("%s: parent cgc1 clk %d\n", __func__, *src);
 
@@ -412,7 +532,7 @@ u32 pcc_clock_get_rate(int pcc_controller, int pcc_clk_slot)
 {
 	u32 val, rate, frac, div;
 	void __iomem *reg;
-	enum cgc1_clk parent;
+	enum cgc_clk parent;
 	int ret;
 	int clk;
 	struct pcc_entry *pcc_array;
@@ -425,7 +545,7 @@ u32 pcc_clock_get_rate(int pcc_controller, int pcc_clk_slot)
 	if (ret)
 		return 0;
 
-	rate = cgc1_clk_get_rate(parent);
+	rate = cgc_clk_get_rate(parent);
 
 	debug("%s: parent rate %u\n", __func__, rate);
 
diff --git a/arch/arm/mach-imx/imx8ulp/soc.c b/arch/arm/mach-imx/imx8ulp/soc.c
index bba6323f96..934b0ef038 100644
--- a/arch/arm/mach-imx/imx8ulp/soc.c
+++ b/arch/arm/mach-imx/imx8ulp/soc.c
@@ -23,6 +23,8 @@
 #include <dm/uclass.h>
 #include <dm/device.h>
 #include <dm/uclass-internal.h>
+#include <fuse.h>
+#include <thermal.h>
 
 DECLARE_GLOBAL_DATA_PTR;
 
@@ -210,11 +212,27 @@ int print_cpuinfo(void)
 
 	cpurev = get_cpu_rev();
 
-	printf("CPU:   Freescale i.MX%s rev%d.%d at %d MHz\n",
+	printf("CPU:   i.MX%s rev%d.%d at %d MHz\n",
 	       get_imx_type((cpurev & 0xFF000) >> 12),
 	       (cpurev & 0x000F0) >> 4, (cpurev & 0x0000F) >> 0,
 	       mxc_get_clock(MXC_ARM_CLK) / 1000000);
 
+#if defined(CONFIG_IMX_PMC_TEMPERATURE)
+	struct udevice *udev;
+	int ret, temp;
+
+	ret = uclass_get_device(UCLASS_THERMAL, 0, &udev);
+	if (!ret) {
+		ret = thermal_get_temp(udev, &temp);
+		if (!ret)
+			printf("CPU current temperature: %d\n", temp);
+		else
+			debug(" - failed to get CPU current temperature\n");
+	} else {
+		debug(" - failed to get CPU current temperature\n");
+	}
+#endif
+
 	printf("Reset cause: %s\n", get_reset_cause(cause));
 
 	printf("Boot mode: ");
@@ -462,28 +480,84 @@ static int trdc_set_access(void)
 	/* Iomuxc0: : PBridge1 slot 33 */
 	trdc_mbc_set_access(2, 7, 1, 33, false);
 
+	/* flexspi0 */
+	trdc_mrc_region_set_access(0, 7, 0x04000000, 0x0c000000, false);
+
+	/* tpm0: PBridge1 slot 21 */
+	trdc_mbc_set_access(2, 7, 1, 21, false);
+	/* lpi2c0: PBridge1 slot 24 */
+	trdc_mbc_set_access(2, 7, 1, 24, false);
 	return 0;
 }
 
+void lpav_configure(void)
+{
+	/* LPAV to APD */
+	setbits_le32(SIM_SEC_BASE_ADDR + 0x44, BIT(7));
+
+	/* PXP/GPU 2D/3D/DCNANO/MIPI_DSI/EPDC/HIFI4 to APD */
+	setbits_le32(SIM_SEC_BASE_ADDR + 0x4c, 0x7F);
+
+	/* LPAV slave/dma2 ch allocation and request allocation to APD */
+	writel(0x1f, SIM_SEC_BASE_ADDR + 0x50);
+	writel(0xffffffff, SIM_SEC_BASE_ADDR + 0x54);
+	writel(0x003fffff, SIM_SEC_BASE_ADDR + 0x58);
+}
+
+void load_lposc_fuse(void)
+{
+	int ret;
+	u32 val = 0, val2 = 0, reg;
+
+	ret = fuse_read(25, 0, &val);
+	if (ret)
+		return; /* failed */
+
+	ret = fuse_read(25, 1, &val2);
+	if (ret)
+		return; /* failed */
+
+	/* LPOSCCTRL */
+	reg = readl(0x2802f304);
+	reg &= ~0xff;
+	reg |= (val & 0xff);
+	writel(reg, 0x2802f304);
+}
+
+void set_lpav_qos(void)
+{
+	/* Set read QoS of dcnano on LPAV NIC */
+	writel(0xf, 0x2e447100);
+}
+
 int arch_cpu_init(void)
 {
 	if (IS_ENABLED(CONFIG_SPL_BUILD)) {
+		u32 val = 0;
+		int ret;
+		bool rdc_en = true; /* Default assume DBD_EN is set */
+
 		/* Disable wdog */
 		init_wdog();
 
+		/* Read DBD_EN fuse */
+		ret = fuse_read(8, 1, &val);
+		if (!ret)
+			rdc_en = !!(val & 0x4000);
+
 		if (get_boot_mode() == SINGLE_BOOT) {
-			release_rdc(RDC_TRDC);
+			if (rdc_en)
+				release_rdc(RDC_TRDC);
+
 			trdc_set_access();
-			/* LPAV to APD */
-			setbits_le32(0x2802B044, BIT(7));
-			/* GPU 2D/3D to APD */
-			setbits_le32(0x2802B04C, BIT(1) | BIT(2));
-			/* DCNANO and MIPI_DSI to APD */
-			setbits_le32(0x2802B04C, BIT(1) | BIT(2) | BIT(3) | BIT(4));
+
+			lpav_configure();
 		}
 
-		/* release xrdc, then allow A35 to write SRAM2 */
-		release_rdc(RDC_XRDC);
+		/* Release xrdc, then allow A35 to write SRAM2 */
+		if (rdc_en)
+			release_rdc(RDC_XRDC);
+
 		xrdc_mrc_region_set_access(2, CONFIG_SPL_TEXT_BASE, 0xE00);
 
 		clock_init();
@@ -531,7 +605,30 @@ __weak void __noreturn jump_to_image_no_args(struct spl_image_info *spl_image)
 
 void imx_get_mac_from_fuse(int dev_id, unsigned char *mac)
 {
+	u32 val[2] = {};
+	int ret;
+
+	ret = fuse_read(5, 3, &val[0]);
+	if (ret)
+		goto err;
+
+	ret = fuse_read(5, 4, &val[1]);
+	if (ret)
+		goto err;
+
+	mac[0] = val[0];
+	mac[1] = val[0] >> 8;
+	mac[2] = val[0] >> 16;
+	mac[3] = val[0] >> 24;
+	mac[4] = val[1];
+	mac[5] = val[1] >> 8;
+
+	debug("%s: MAC%d: %02x.%02x.%02x.%02x.%02x.%02x\n",
+	      __func__, dev_id, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+	return;
+err:
 	memset(mac, 0, 6);
+	printf("%s: fuse read err: %d\n", __func__, ret);
 }
 
 int (*card_emmc_is_boot_part_en)(void) = (void *)0x67cc;