imx: imx9: Add CCM and clock API support

Add clock API to support CCM root clock and LPCG setting
Set the CCM AUTHEN register to allow non-secure world to set
root clock and lpcg.

Signed-off-by: Ye Li <ye.li@nxp.com>
Signed-off-by: Peng Fan <peng.fan@nxp.com>

arch/arm/include/asm/arch-imx9/ccm_regs.h

@@ -0,0 +1,266 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2022 NXP
+ */
+
+#ifndef __ASM_ARCH_IMX9_CCM_REGS_H__
+#define __ASM_ARCH_IMX9_CCM_REGS_H__
+#define IMX93_CLK_ROOT_MAX	95
+#define IMX93_CLK_CCGR_MAX	127
+
+#define ARM_A55_PERIPH_CLK_ROOT		0
+#define ARM_A55_MTR_BUS_CLK_ROOT	1
+#define ARM_A55_CLK_ROOT		2
+#define M33_CLK_ROOT			3
+#define SENTINEL_CLK_ROOT		4
+#define BUS_WAKEUP_CLK_ROOT		5
+#define BUS_AON_CLK_ROOT		6
+#define WAKEUP_AXI_CLK_ROOT		7
+#define SWO_TRACE_CLK_ROOT		8
+#define M33_SYSTICK_CLK_ROOT		9
+#define FLEXIO1_CLK_ROOT		10
+#define FLEXIO2_CLK_ROOT		11
+#define LPIT1_CLK_ROOT			12
+#define LPIT2_CLK_ROOT			13
+#define LPTMR1_CLK_ROOT			14
+#define LPTMR2_CLK_ROOT			15
+#define TPM1_CLK_ROOT			16
+#define TPM2_CLK_ROOT			17
+#define TPM3_CLK_ROOT			18
+#define TPM4_CLK_ROOT			19
+#define TPM5_CLK_ROOT			20
+#define TPM6_CLK_ROOT			21
+#define FLEXSPI1_CLK_ROOT		22
+#define CAN1_CLK_ROOT			23
+#define CAN2_CLK_ROOT			24
+#define LPUART1_CLK_ROOT		25
+#define LPUART2_CLK_ROOT		26
+#define LPUART3_CLK_ROOT		27
+#define LPUART4_CLK_ROOT		28
+#define LPUART5_CLK_ROOT		29
+#define LPUART6_CLK_ROOT		30
+#define LPUART7_CLK_ROOT		31
+#define LPUART8_CLK_ROOT		32
+#define LPI2C1_CLK_ROOT			33
+#define LPI2C2_CLK_ROOT			34
+#define LPI2C3_CLK_ROOT			35
+#define LPI2C4_CLK_ROOT			36
+#define LPI2C5_CLK_ROOT			37
+#define LPI2C6_CLK_ROOT			38
+#define LPI2C7_CLK_ROOT			39
+#define LPI2C8_CLK_ROOT			40
+#define LPSPI1_CLK_ROOT			41
+#define LPSPI2_CLK_ROOT			42
+#define LPSPI3_CLK_ROOT			43
+#define LPSPI4_CLK_ROOT			44
+#define LPSPI5_CLK_ROOT			45
+#define LPSPI6_CLK_ROOT			46
+#define LPSPI7_CLK_ROOT			47
+#define LPSPI8_CLK_ROOT			48
+#define I3C1_CLK_ROOT			49
+#define I3C2_CLK_ROOT			50
+#define USDHC1_CLK_ROOT			51
+#define USDHC2_CLK_ROOT			52
+#define USDHC3_CLK_ROOT			53
+#define SAI1_CLK_ROOT			54
+#define SAI2_CLK_ROOT			55
+#define SAI3_CLK_ROOT			56
+#define CCM_CKO1_CLK_ROOT		57
+#define CCM_CKO2_CLK_ROOT		58
+#define CCM_CKO3_CLK_ROOT		59
+#define CCM_CKO4_CLK_ROOT		60
+#define HSIO_CLK_ROOT			61
+#define HSIO_USB_TEST_60M_CLK_ROOT	62
+#define HSIO_ACSCAN_80M_CLK_ROOT	63
+#define HSIO_ACSCAN_480M_CLK_ROOT	64
+#define NIC_CLK_ROOT			65
+#define NIC_APB_CLK_ROOT		66
+#define ML_APB_CLK_ROOT			67
+#define ML_CLK_ROOT			68
+#define MEDIA_AXI_CLK_ROOT		69
+#define MEDIA_APB_CLK_ROOT		70
+#define MEDIA_LDB_CLK_ROOT		71
+#define MEDIA_DISP_PIX_CLK_ROOT		72
+#define CAM_PIX_CLK_ROOT		73
+#define MIPI_TEST_BYTE_CLK_ROOT		74
+#define MIPI_PHY_CFG_CLK_ROOT		75
+#define DRAM_ALT_CLK_ROOT		76
+#define DRAM_APB_CLK_ROOT		77
+#define ADC_CLK_ROOT			78
+#define PDM_CLK_ROOT			79
+#define TSTMR1_CLK_ROOT			80
+#define TSTMR2_CLK_ROOT			81
+#define MQS1_CLK_ROOT			82
+#define MQS2_CLK_ROOT			83
+#define AUDIO_XCVR_CLK_ROOT		84
+#define SPDIF_CLK_ROOT			85
+#define ENET_CLK_ROOT			86
+#define ENET_TIMER1_CLK_ROOT		87
+#define ENET_TIMER2_CLK_ROOT		88
+#define ENET_REF_CLK_ROOT		89
+#define ENET_REF_PHY_CLK_ROOT		90
+#define I3C1_SLOW_CLK_ROOT		91
+#define I3C2_SLOW_CLK_ROOT		92
+#define USB_PHY_BURUNIN_CLK_ROOT	93
+#define PAL_CAME_SCAN_CLK_ROOT	94
+#define CLK_ROOT_NUM			95
+
+#define CCGR_A55	0
+#define CCGR_CM33	1
+#define CCGR_ARMTROUT	2
+#define CCGR_SENT	3
+#define CCGR_BUSM	4
+#define CCGR_BUS7	5
+#define CCGR_BUSD	6
+#define CCGR_ANAD	7
+#define CCGR_SRC	8
+#define CCGR_CCM	9
+#define CCGR_GPC	10
+#define CCGR_ADC	11
+#define CCGR_WDG1	12
+#define CCGR_WDG2	13
+#define CCGR_WDG3	14
+#define CCGR_WDG4	15
+#define CCGR_WDG5	16
+#define CCGR_SEM1	17
+#define CCGR_SEM2	18
+#define CCGR_MUA	19
+#define CCGR_MUB	20
+#define CCGR_DMA1	21
+#define CCGR_DMA2	22
+#define CCGR_ROMCA55	23
+#define CCGR_ROMCM33	24
+#define CCGR_QSP1	25
+#define CCGR_AONRDC	26
+#define CCGR_WKUPRDC	27
+#define CCGR_FUSE	28
+#define CCGR_SNVH	29
+#define CCGR_SNVS	30
+#define CCGR_TRAC	31
+#define CCGR_SWO	32
+#define CCGR_IOCG	33
+#define CCGR_PIO1	34
+#define CCGR_PIO2	35
+#define CCGR_PIO3	36
+#define CCGR_PIO4	37
+#define CCGR_FIO1	38
+#define CCGR_FIO2	39
+#define CCGR_PIT1	40
+#define CCGR_PIT2	41
+#define CCGR_GPT1	42
+#define CCGR_GPT2	43
+#define CCGR_TPM1	44
+#define CCGR_TPM2	45
+#define CCGR_TPM3	46
+#define CCGR_TPM4	47
+#define CCGR_TPM5	48
+#define CCGR_TPM6	49
+#define CCGR_CAN1	50
+#define CCGR_CAN2	51
+#define CCGR_URT1	52
+#define CCGR_URT2	53
+#define CCGR_URT3	54
+#define CCGR_URT4	55
+#define CCGR_URT5	56
+#define CCGR_URT6	57
+#define CCGR_URT7	58
+#define CCGR_URT8	59
+#define CCGR_I2C1	60
+#define CCGR_I2C2	61
+#define CCGR_I2C3	62
+#define CCGR_I2C4	63
+#define CCGR_I2C5	64
+#define CCGR_I2C6	65
+#define CCGR_I2C7	66
+#define CCGR_I2C8	67
+#define CCGR_SPI1	68
+#define CCGR_SPI2	69
+#define CCGR_SPI3	70
+#define CCGR_SPI4	71
+#define CCGR_SPI5	72
+#define CCGR_SPI6	73
+#define CCGR_SPI7	74
+#define CCGR_SPI8	75
+#define CCGR_I3C1	76
+#define CCGR_I3C2	77
+#define CCGR_USDHC1	78
+#define CCGR_USDHC2	79
+#define CCGR_USDHC3	80
+#define CCGR_SAI1	81
+#define CCGR_SAI2	82
+#define CCGR_SAI3	83
+#define CCGR_W2AO	84
+#define CCGR_AO2W	85
+#define CCGR_MIPIC	86
+#define CCGR_MIPID	87
+#define CCGR_LVDS	88
+#define CCGR_LCDIF	89
+#define CCGR_PXP	90
+#define CCGR_ISI	91
+#define CCGR_NMED	92
+#define CCGR_DFI	93
+#define CCGR_DDRC	94
+#define CCGR_DFIC	95
+#define CCGR_DSSI	96
+#define CCGR_DBYP	97
+#define CCGR_DAPB	98
+#define CCGR_DRAMP	99
+#define CCGR_DCLKC	100
+#define CCGR_NCTL	101
+#define CCGR_GIC	102
+#define CCGR_NICAPB	103
+#define CCGR_USBC	104
+#define CCGR_USBT	105
+#define CCGR_HSIO	106
+#define CCGR_PDM	107
+#define CCGR_MQS1	108
+#define CCGR_MQS2	109
+#define CCGR_AXCVR	110
+#define CCGR_MECC	111
+#define CCGR_SPDIF	112
+#define CCGR_ML2NIC	113
+#define CCGR_MED2NIC	114
+#define CCGR_HSIO2NIC	115
+#define CCGR_W2NIC	116
+#define CCGR_NIC2W	117
+#define CCGR_NIC2DDR	118
+#define CCGR_HSIO32K	119
+#define CCGR_ENET1	120
+#define CCGR_ENETQOS	121
+#define CCGR_SYSCNT	122
+#define CCGR_TSTMR1	123
+#define CCGR_TSTMR2	124
+#define CCGR_TMC	125
+#define CCGR_PMRO	126
+#define CCGR_NUM	127
+
+#define SHARED_GPR_EXT_CLK 0
+#define SHARED_GPR_EXT_CLK_SEL_EXT1 0
+#define SHARED_GPR_EXT_CLK_SEL_EXT2 BIT(0)
+#define SHARED_GPR_EXT_CLK_SEL_EXT3 BIT(1)
+#define SHARED_GPR_EXT_CLK_SEL_EXT4 GENMASK(1, 0)
+
+#define SHARED_GPR_A55_CLK 1
+#define SHARED_GPR_A55_CLK_SEL_CCM 0
+#define SHARED_GPR_A55_CLK_SEL_PLL BIT(0)
+
+#define SHARED_GPR_DRAM_CLK 2
+#define SHARED_GPR_DRAM_CLK_SEL_PLL 0
+#define SHARED_GPR_DRAM_CLK_SEL_CCM BIT(0)
+
+#define SHARED_GPR_NUM 8
+#define PRIVATE_GPR_NUM 8
+
+#define CLK_ROOT_STATUS_OFF		BIT(24)
+#define CLK_ROOT_STATUS_CHANGING	BIT(31)
+#define CLK_ROOT_MUX_MASK		GENMASK(9, 8)
+#define CLK_ROOT_MUX_SHIFT		8
+#define CLK_ROOT_DIV_MASK		GENMASK(7, 0)
+
+#define CCM_AUTHEN_LOCK_TZ		BIT(11)
+#define CCM_AUTHEN_TZ_NS		BIT(9)
+#define CCM_AUTHEN_TZ_USER		BIT(8)
+#define CCM_AUTHEN_CPULPM_MODE	BIT(2)
+#define CCM_AUTHEN_AUTO_CTRL	BIT(3)
+
+#endif

arch/arm/include/asm/arch-imx9/clock.h

@@ -0,0 +1,239 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2022 NXP
+ *
+ * Peng Fan <peng.fan at nxp.com>
+ */
+
+#ifndef __CLOCK_IMX9__
+#define __CLOCK_IMX9__
+
+#include <linux/bitops.h>
+
+#define MHZ(x)	((x) * 1000000UL)
+
+enum enet_freq {
+	ENET_25MHZ = 0,
+	ENET_50MHZ,
+	ENET_125MHZ,
+};
+
+enum ccm_clk_src {
+	OSC_24M_CLK,
+	ARM_PLL,
+	ARM_PLL_CLK,
+	SYS_PLL_PG,
+	SYS_PLL_PFD0_PG,
+	SYS_PLL_PFD0,
+	SYS_PLL_PFD0_DIV2,
+	SYS_PLL_PFD1_PG,
+	SYS_PLL_PFD1,
+	SYS_PLL_PFD1_DIV2,
+	SYS_PLL_PFD2_PG,
+	SYS_PLL_PFD2,
+	SYS_PLL_PFD2_DIV2,
+	AUDIO_PLL,
+	AUDIO_PLL_CLK,
+	DRAM_PLL,
+	DRAM_PLL_CLK,
+	VIDEO_PLL,
+	VIDEO_PLL_CLK,
+	OSCPLL_END,
+	EXT_CLK,
+};
+
+/* Mainly for compatible to imx common code. */
+enum mxc_clock {
+	MXC_ARM_CLK = 0,
+	MXC_IPG_CLK,
+	MXC_FLEXSPI_CLK,
+	MXC_CSPI_CLK,
+	MXC_ESDHC_CLK,
+	MXC_ESDHC2_CLK,
+	MXC_ESDHC3_CLK,
+	MXC_UART_CLK,
+	MXC_I2C_CLK,
+	MXC_FEC_CLK,
+};
+
+struct ccm_obs {
+	u32 direct;
+	u32 reserved[31];
+};
+
+struct ccm_gpr {
+	u32 gpr;
+	u32 gpr_set;
+	u32 gpr_clr;
+	u32 gpr_tog;
+	u32 authen;
+	u32 authen_set;
+	u32 authen_clr;
+	u32 authen_tog;
+};
+
+struct ccm_lpcg_oscpll {
+	u32 direct;
+	u32 lpm_status0;
+	u32 lpm_status1;
+	u32 reserved0;
+	u32 lpm0;
+	u32 lpm1;
+	u32 reserved1;
+	u32 lpm_cur;
+	u32 status0;
+	u32 status1;
+	u32 reserved2[2];
+	u32 authen;
+	u32 reserved3[3];
+};
+
+struct ccm_root {
+	u32 control;
+	u32 control_set;
+	u32 control_clr;
+	u32 control_tog;
+	u32 reserved[4];
+	u32 status0;
+	u32 reserved1[3];
+	u32 authen;
+	u32 reserved2[19];
+};
+
+struct ccm_reg {
+	struct ccm_root clk_roots[95]; /* 0x0 */
+	u32 reserved_0[1312];
+	struct ccm_obs clk_obs[6]; /* 0x4400 */
+	u32 reserved_1[64];
+	struct ccm_gpr clk_shared_gpr[8]; /* 0x4800 */
+	u32 reserved_2[192];
+	struct ccm_gpr clk_private_gpr[8]; /* 0x4C00 */
+	u32 reserved_3[192];
+	struct ccm_lpcg_oscpll clk_oscplls[19]; /* 0x5000 */
+	u32 reserved_4[2768];
+	struct ccm_lpcg_oscpll clk_lpcgs[122]; /* 0x8000 */
+};
+
+struct ana_pll_reg_elem {
+	u32 reg;
+	u32 reg_set;
+	u32 reg_clr;
+	u32 reg_tog;
+};
+
+struct ana_pll_dfs {
+	struct ana_pll_reg_elem dfs_ctrl;
+	struct ana_pll_reg_elem dfs_div;
+};
+
+struct ana_pll_reg {
+	struct ana_pll_reg_elem ctrl;
+	struct ana_pll_reg_elem ana_prg;
+	struct ana_pll_reg_elem test;
+	struct ana_pll_reg_elem ss; /* Spread spectrum */
+	struct ana_pll_reg_elem num; /* numerator */
+	struct ana_pll_reg_elem denom; /* demoninator */
+	struct ana_pll_reg_elem div;
+	struct ana_pll_dfs dfs[4];
+	u32 pll_status;
+	u32 dfs_status;
+	u32 reserved[2];
+};
+
+struct anatop_reg {
+	u32 osc_ctrl;
+	u32 osc_state;
+	u32 reserved_0[510];
+	u32 chip_version;
+	u32 reserved_1[511];
+	struct ana_pll_reg arm_pll;
+	struct ana_pll_reg sys_pll;
+	struct ana_pll_reg audio_pll;
+	struct ana_pll_reg dram_pll;
+	struct ana_pll_reg video_pll;
+};
+
+#define PLL_CTRL_HW_CTRL_SEL BIT(16)
+#define PLL_CTRL_CLKMUX_BYPASS BIT(2)
+#define PLL_CTRL_CLKMUX_EN BIT(1)
+#define PLL_CTRL_POWERUP BIT(0)
+
+#define PLL_STATUS_PLL_LOCK BIT(0)
+#define PLL_DFS_CTRL_ENABLE BIT(31)
+#define PLL_DFS_CTRL_CLKOUT BIT(30)
+#define PLL_DFS_CTRL_CLKOUT_DIV2 BIT(29)
+#define PLL_DFS_CTRL_BYPASS BIT(23)
+
+#define PLL_SS_EN BIT(15)
+
+struct imx_intpll_rate_table {
+	u32 rate; /*khz*/
+	int rdiv;
+	int mfi;
+	int odiv;
+};
+
+struct imx_fracpll_rate_table {
+	u32 rate; /*khz*/
+	int rdiv;
+	int mfi;
+	int odiv;
+	int mfn;
+	int mfd;
+};
+
+#define INT_PLL_RATE(_rate, _r, _m, _o)			\
+	{							\
+		.rate	=	(_rate),			\
+		.rdiv	=	(_r),				\
+		.mfi	=	(_m),				\
+		.odiv	=	(_o),				\
+	}
+
+#define FRAC_PLL_RATE(_rate, _r, _m, _o, _n, _d)			\
+	{							\
+		.rate	=	(_rate),			\
+		.rdiv	=	(_r),				\
+		.mfi	=	(_m),				\
+		.odiv	=	(_o),				\
+		.mfn	=	(_n),				\
+		.mfd	=	(_d),				\
+	}
+
+struct clk_root_map {
+	u32 clk_root_id;
+	u32 mux_type;
+};
+
+int clock_init(void);
+u32 get_clk_src_rate(enum ccm_clk_src source);
+u32 get_lpuart_clk(void);
+void init_uart_clk(u32 index);
+void init_clk_usdhc(u32 index);
+int enable_i2c_clk(unsigned char enable, u32 i2c_num);
+u32 imx_get_i2cclk(u32 i2c_num);
+u32 mxc_get_clock(enum mxc_clock clk);
+
+int ccm_clk_src_on(enum ccm_clk_src oscpll, bool enable);
+int ccm_clk_src_auto(enum ccm_clk_src oscpll, bool enable);
+int ccm_clk_src_lpm(enum ccm_clk_src oscpll, bool enable);
+int ccm_clk_src_config_lpm(enum ccm_clk_src oscpll, u32 domain, u32 lpm_val);
+bool ccm_clk_src_is_clk_on(enum ccm_clk_src oscpll);
+int ccm_clk_src_tz_access(enum ccm_clk_src oscpll, bool non_secure, bool user_mode, bool lock_tz);
+int ccm_clk_root_cfg(u32 clk_root_id, enum ccm_clk_src src, u32 div);
+u32 ccm_clk_root_get_rate(u32 clk_root_id);
+int ccm_clk_root_tz_access(u32 clk_root_id, bool non_secure, bool user_mode, bool lock_tz);
+int ccm_lpcg_on(u32 lpcg, bool enable);
+int ccm_lpcg_lpm(u32 lpcg, bool enable);
+int ccm_lpcg_config_lpm(u32 lpcg, u32 domain, u32 lpm_val);
+bool ccm_lpcg_is_clk_on(u32 lpcg);
+int ccm_lpcg_tz_access(u32 lpcg, bool non_secure, bool user_mode, bool lock_tz);
+int ccm_shared_gpr_set(u32 gpr, u32 val);
+int ccm_shared_gpr_get(u32 gpr, u32 *val);
+int ccm_shared_gpr_tz_access(u32 gpr, bool non_secure, bool user_mode, bool lock_tz);
+
+void enable_usboh3_clk(unsigned char enable);
+int set_clk_enet(enum enet_freq type);
+int set_clk_eqos(enum enet_freq type);
+
+#endif

arch/arm/include/asm/arch-imx9/imx-regs.h

@@ -8,6 +8,10 @@
 
 #define ARCH_MXC
 
-#define IOMUXC_BASE_ADDR 0x443C0000UL
+#define IOMUXC_BASE_ADDR	0x443C0000UL
+#define CCM_BASE_ADDR		0x44450000UL
+#define CCM_CCGR_BASE_ADDR	0x44458000UL
+
+#define ANATOP_BASE_ADDR    0x44480000UL
 
 #endif

arch/arm/mach-imx/imx9/Makefile

@@ -3,4 +3,4 @@
 # Copyright 2022 NXP
 
 obj-y += lowlevel_init.o
-obj-y += soc.o clock.o
+obj-y += soc.o clock.o clock_root.o

arch/arm/mach-imx/imx9/clock.c

@@ -9,6 +9,7 @@
 #include <command.h>
 #include <asm/arch/clock.h>
 #include <asm/arch/imx-regs.h>
+#include <asm/arch/ccm_regs.h>
 #include <asm/arch/sys_proto.h>
 #include <asm/global_data.h>
 #include <asm/io.h>
@@ -20,7 +21,773 @@
 
 DECLARE_GLOBAL_DATA_PTR;
 
+static struct anatop_reg *ana_regs = (struct anatop_reg *)ANATOP_BASE_ADDR;
+
+static struct imx_intpll_rate_table imx9_intpll_tbl[] = {
+	INT_PLL_RATE(1800000000U, 1, 150, 2), /* 1.8Ghz */
+	INT_PLL_RATE(1700000000U, 1, 141, 2), /* 1.7Ghz */
+	INT_PLL_RATE(1400000000U, 1, 175, 3), /* 1.4Ghz */
+	INT_PLL_RATE(1000000000U, 1, 166, 4), /* 1000Mhz */
+	INT_PLL_RATE(900000000U, 1, 150, 4), /* 900Mhz */
+};
+
+static struct imx_fracpll_rate_table imx9_fracpll_tbl[] = {
+	FRAC_PLL_RATE(1000000000U, 1, 166, 4, 2, 3), /* 1000Mhz */
+	FRAC_PLL_RATE(933000000U, 1, 155, 4, 1, 2), /* 933Mhz */
+	FRAC_PLL_RATE(700000000U, 1, 145, 5, 5, 6), /* 700Mhz */
+	FRAC_PLL_RATE(466000000U, 1, 155, 8, 1, 3), /* 466Mhz */
+	FRAC_PLL_RATE(400000000U, 1, 200, 12, 0, 1), /* 400Mhz */
+};
+
+/* return in khz */
+static u32 decode_pll_vco(struct ana_pll_reg *reg, bool fracpll)
+{
+	u32 ctrl;
+	u32 pll_status;
+	u32 div;
+	int rdiv, mfi, mfn, mfd;
+	int clk = 24000;
+
+	ctrl = readl(&reg->ctrl.reg);
+	pll_status = readl(&reg->pll_status);
+	div = readl(&reg->div.reg);
+
+	if (!(ctrl & PLL_CTRL_POWERUP))
+		return 0;
+
+	if (!(pll_status & PLL_STATUS_PLL_LOCK))
+		return 0;
+
+	mfi = (div & GENMASK(24, 16)) >> 16;
+	rdiv = (div & GENMASK(15, 13)) >> 13;
+
+	if (rdiv == 0)
+		rdiv = 1;
+
+	if (fracpll) {
+		mfn = (int)readl(&reg->num.reg);
+		mfn >>= 2;
+		mfd = (int)(readl(&reg->denom.reg) & GENMASK(29, 0));
+
+		clk = clk * (mfi * mfd + mfn) / mfd / rdiv;
+	} else {
+		clk = clk * mfi / rdiv;
+	}
+
+	return (u32)clk;
+}
+
+/* return in khz */
+static u32 decode_pll_out(struct ana_pll_reg *reg, bool fracpll)
+{
+	u32 ctrl = readl(&reg->ctrl.reg);
+	u32 div;
+
+	if (ctrl & PLL_CTRL_CLKMUX_BYPASS)
+		return 24000;
+
+	if (!(ctrl & PLL_CTRL_CLKMUX_EN))
+		return 0;
+
+	div = readl(&reg->div.reg);
+	div &= 0xff; /* odiv */
+
+	if (div == 0)
+		div = 2;
+	else if (div == 1)
+		div = 3;
+
+	return decode_pll_vco(reg, fracpll) / div;
+}
+
+/* return in khz */
+static u32 decode_pll_pfd(struct ana_pll_reg *reg, struct ana_pll_dfs *dfs_reg,
+			  bool div2, bool fracpll)
+{
+	u32 pllvco = decode_pll_vco(reg, fracpll);
+	u32 dfs_ctrl = readl(&dfs_reg->dfs_ctrl.reg);
+	u32 dfs_div = readl(&dfs_reg->dfs_div.reg);
+	u32 mfn, mfi;
+	u32 output;
+
+	if (dfs_ctrl & PLL_DFS_CTRL_BYPASS)
+		return pllvco;
+
+	if (!(dfs_ctrl & PLL_DFS_CTRL_ENABLE) ||
+	    (div2 && !(dfs_ctrl & PLL_DFS_CTRL_CLKOUT_DIV2)) ||
+	    (!div2 && !(dfs_ctrl & PLL_DFS_CTRL_CLKOUT)))
+		return 0;
+
+	mfn = dfs_div & GENMASK(2, 0);
+	mfi = (dfs_div & GENMASK(15, 8)) >> 8;
+
+	if (mfn > 3)
+		return 0; /* valid mfn 0-3 */
+
+	if (mfi == 0 || mfi == 1)
+		return 0; /* valid mfi 2-255 */
+
+	output = (pllvco * 5) / (mfi * 5 + mfn);
+
+	if (div2)
+		return output >> 1;
+
+	return output;
+}
+
+static u32 decode_pll(enum ccm_clk_src pll)
+{
+	switch (pll) {
+	case ARM_PLL_CLK:
+		return decode_pll_out(&ana_regs->arm_pll, false);
+	case SYS_PLL_PG:
+		return decode_pll_out(&ana_regs->sys_pll, false);
+	case SYS_PLL_PFD0:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[0], false, true);
+	case SYS_PLL_PFD0_DIV2:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[0], true, true);
+	case SYS_PLL_PFD1:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[1], false, true);
+	case SYS_PLL_PFD1_DIV2:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[1], true, true);
+	case SYS_PLL_PFD2:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[2], false, true);
+	case SYS_PLL_PFD2_DIV2:
+		return decode_pll_pfd(&ana_regs->sys_pll,
+			&ana_regs->sys_pll.dfs[2], true, true);
+	case AUDIO_PLL_CLK:
+		return decode_pll_out(&ana_regs->audio_pll, true);
+	case DRAM_PLL_CLK:
+		return decode_pll_out(&ana_regs->dram_pll, true);
+	case VIDEO_PLL_CLK:
+		return decode_pll_out(&ana_regs->video_pll, true);
+	default:
+		printf("Invalid clock source to decode\n");
+		break;
+	}
+
+	return 0;
+}
+
+int configure_intpll(enum ccm_clk_src pll, u32 freq)
+{
+	int i;
+	struct imx_intpll_rate_table *rate;
+	struct ana_pll_reg *reg;
+	u32 pll_status;
+
+	for (i = 0; i < ARRAY_SIZE(imx9_intpll_tbl); i++) {
+		if (freq == imx9_intpll_tbl[i].rate)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(imx9_intpll_tbl)) {
+		debug("No matched freq table %u\n", freq);
+		return -EINVAL;
+	}
+
+	rate = &imx9_intpll_tbl[i];
+
+	/* ROM has configured SYS PLL and PFD, no need for it */
+	switch (pll) {
+	case ARM_PLL_CLK:
+		reg = &ana_regs->arm_pll;
+		break;
+	default:
+		return -EPERM;
+	}
+
+	/* Bypass the PLL to ref */
+	writel(PLL_CTRL_CLKMUX_BYPASS, &reg->ctrl.reg_set);
+
+	/* disable pll and output */
+	writel(PLL_CTRL_CLKMUX_EN | PLL_CTRL_POWERUP, &reg->ctrl.reg_clr);
+
+	/* Program the ODIV, RDIV, MFI */
+	writel((rate->odiv & GENMASK(7, 0)) | ((rate->rdiv << 13) & GENMASK(15, 13)) |
+	       ((rate->mfi << 16) & GENMASK(24, 16)), &reg->div.reg);
+
+	/* wait 5us */
+	udelay(5);
+
+	/* power up the PLL and wait lock (max wait time 100 us) */
+	writel(PLL_CTRL_POWERUP, &reg->ctrl.reg_set);
+
+	udelay(100);
+
+	pll_status = readl(&reg->pll_status);
+	if (pll_status & PLL_STATUS_PLL_LOCK) {
+		writel(PLL_CTRL_CLKMUX_EN, &reg->ctrl.reg_set);
+
+		/* clear bypass */
+		writel(PLL_CTRL_CLKMUX_BYPASS, &reg->ctrl.reg_clr);
+
+	} else {
+		debug("Fail to lock PLL %u\n", pll);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int configure_fracpll(enum ccm_clk_src pll, u32 freq)
+{
+	struct imx_fracpll_rate_table *rate;
+	struct ana_pll_reg *reg;
+	u32 pll_status;
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(imx9_fracpll_tbl); i++) {
+		if (freq == imx9_fracpll_tbl[i].rate)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(imx9_fracpll_tbl)) {
+		debug("No matched freq table %u\n", freq);
+		return -EINVAL;
+	}
+
+	rate = &imx9_fracpll_tbl[i];
+
+	switch (pll) {
+	case SYS_PLL_PG:
+		reg = &ana_regs->sys_pll;
+		break;
+	case DRAM_PLL_CLK:
+		reg = &ana_regs->dram_pll;
+		break;
+	case VIDEO_PLL_CLK:
+		reg = &ana_regs->video_pll;
+		break;
+	default:
+		return -EPERM;
+	}
+
+	/* Bypass the PLL to ref */
+	writel(PLL_CTRL_CLKMUX_BYPASS, &reg->ctrl.reg_set);
+
+	/* disable pll and output */
+	writel(PLL_CTRL_CLKMUX_EN | PLL_CTRL_POWERUP, &reg->ctrl.reg_clr);
+
+	/* Program the ODIV, RDIV, MFI */
+	writel((rate->odiv & GENMASK(7, 0)) | ((rate->rdiv << 13) & GENMASK(15, 13)) |
+	       ((rate->mfi << 16) & GENMASK(24, 16)), &reg->div.reg);
+
+	/* Set SPREAD_SPECRUM enable to 0 */
+	writel(PLL_SS_EN, &reg->ss.reg_clr);
+
+	/* Program NUMERATOR and DENOMINATOR */
+	writel((rate->mfn << 2), &reg->num.reg);
+	writel((rate->mfd & GENMASK(29, 0)), &reg->denom.reg);
+
+	/* wait 5us */
+	udelay(5);
+
+	/* power up the PLL and wait lock (max wait time 100 us) */
+	writel(PLL_CTRL_POWERUP, &reg->ctrl.reg_set);
+
+	udelay(100);
+
+	pll_status = readl(&reg->pll_status);
+	if (pll_status & PLL_STATUS_PLL_LOCK) {
+		writel(PLL_CTRL_CLKMUX_EN, &reg->ctrl.reg_set);
+
+		/* check the MFN is updated */
+		pll_status = readl(&reg->pll_status);
+		if ((pll_status & ~0x3) != (rate->mfn << 2)) {
+			debug("MFN update not matched, pll_status 0x%x, mfn 0x%x\n",
+			      pll_status, rate->mfn);
+			return -EIO;
+		}
+
+		/* clear bypass */
+		writel(PLL_CTRL_CLKMUX_BYPASS, &reg->ctrl.reg_clr);
+
+	} else {
+		debug("Fail to lock PLL %u\n", pll);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+int configure_pll_pfd(enum ccm_clk_src pll_pfg, u32 mfi, u32 mfn, bool div2_en)
+{
+	struct ana_pll_dfs *dfs;
+	struct ana_pll_reg *reg;
+	u32 dfs_status;
+	u32 index;
+
+	if (mfn > 3)
+		return -EINVAL; /* valid mfn 0-3 */
+
+	if (mfi < 2 || mfi > 255)
+		return -EINVAL; /* valid mfi 2-255 */
+
+	switch (pll_pfg) {
+	case SYS_PLL_PFD0:
+		reg = &ana_regs->sys_pll;
+		index = 0;
+		break;
+	case SYS_PLL_PFD1:
+		reg = &ana_regs->sys_pll;
+		index = 1;
+		break;
+	case SYS_PLL_PFD2:
+		reg = &ana_regs->sys_pll;
+		index = 2;
+		break;
+	default:
+		return -EPERM;
+	}
+
+	dfs = &reg->dfs[index];
+
+	/* Bypass the DFS to PLL VCO */
+	writel(PLL_DFS_CTRL_BYPASS, &dfs->dfs_ctrl.reg_set);
+
+	/* disable DFS and output */
+	writel(PLL_DFS_CTRL_ENABLE | PLL_DFS_CTRL_CLKOUT |
+		PLL_DFS_CTRL_CLKOUT_DIV2, &dfs->dfs_ctrl.reg_clr);
+
+	writel(((mfi << 8) & GENMASK(15, 8)) | (mfn & GENMASK(2, 0)), &dfs->dfs_div.reg);
+
+	writel(PLL_DFS_CTRL_CLKOUT, &dfs->dfs_ctrl.reg_set);
+	if (div2_en)
+		writel(PLL_DFS_CTRL_CLKOUT_DIV2, &dfs->dfs_ctrl.reg_set);
+	writel(PLL_DFS_CTRL_ENABLE, &dfs->dfs_ctrl.reg_set);
+
+	/*
+	 * As HW expert said: after enabling the DFS, clock will start
+	 * coming after 6 cycles output clock period.
+	 * 5us is much bigger than expected, so it will be safe
+	 */
+	udelay(5);
+
+	dfs_status = readl(&reg->dfs_status);
+
+	if (!(dfs_status & (1 << index))) {
+		debug("DFS lock failed\n");
+		return -EIO;
+	}
+
+	/* Bypass the DFS to PLL VCO */
+	writel(PLL_DFS_CTRL_BYPASS, &dfs->dfs_ctrl.reg_clr);
+
+	return 0;
+}
+
+int update_fracpll_mfn(enum ccm_clk_src pll, int mfn)
+{
+	struct ana_pll_reg *reg;
+	bool repoll = false;
+	u32 pll_status;
+	int count = 20;
+
+	switch (pll) {
+	case AUDIO_PLL_CLK:
+		reg = &ana_regs->audio_pll;
+		break;
+	case DRAM_PLL_CLK:
+		reg = &ana_regs->dram_pll;
+		break;
+	case VIDEO_PLL_CLK:
+		reg = &ana_regs->video_pll;
+		break;
+	default:
+		printf("Invalid pll %u for update FRAC PLL MFN\n", pll);
+		return -EINVAL;
+	}
+
+	if (readl(&reg->pll_status) & PLL_STATUS_PLL_LOCK)
+		repoll = true;
+
+	mfn <<= 2;
+	writel(mfn, &reg->num);
+
+	if (repoll) {
+		do {
+			pll_status = readl(&reg->pll_status);
+			udelay(5);
+			count--;
+		} while (((pll_status & ~0x3) != (u32)mfn) && count > 0);
+
+		if (count <= 0) {
+			printf("update MFN timeout, pll_status 0x%x, mfn 0x%x\n", pll_status, mfn);
+			return -EIO;
+		}
+	}
+
+	return 0;
+}
+
+int update_pll_pfd_mfn(enum ccm_clk_src pll_pfd, u32 mfn)
+{
+	struct ana_pll_dfs *dfs;
+	u32 val;
+	u32 index;
+
+	switch (pll_pfd) {
+	case SYS_PLL_PFD0:
+	case SYS_PLL_PFD0_DIV2:
+		index = 0;
+		break;
+	case SYS_PLL_PFD1:
+	case SYS_PLL_PFD1_DIV2:
+		index = 1;
+		break;
+	case SYS_PLL_PFD2:
+	case SYS_PLL_PFD2_DIV2:
+		index = 2;
+		break;
+	default:
+		printf("Invalid pfd %u for update PLL PFD MFN\n", pll_pfd);
+		return -EINVAL;
+	}
+
+	dfs = &ana_regs->sys_pll.dfs[index];
+
+	val = readl(&dfs->dfs_div.reg);
+	val &= ~0x3;
+	val |= mfn & 0x3;
+	writel(val, &dfs->dfs_div.reg);
+
+	return 0;
+}
+
+/* return in khz */
+u32 get_clk_src_rate(enum ccm_clk_src source)
+{
+	u32 ctrl;
+	bool clk_on;
+
+	switch (source) {
+	case ARM_PLL_CLK:
+		ctrl = readl(&ana_regs->arm_pll.ctrl.reg);
+	case AUDIO_PLL_CLK:
+		ctrl = readl(&ana_regs->audio_pll.ctrl.reg);
+		break;
+	case DRAM_PLL_CLK:
+		ctrl = readl(&ana_regs->dram_pll.ctrl.reg);
+		break;
+	case VIDEO_PLL_CLK:
+		ctrl = readl(&ana_regs->video_pll.ctrl.reg);
+		break;
+	case SYS_PLL_PFD0:
+	case SYS_PLL_PFD0_DIV2:
+		ctrl = readl(&ana_regs->sys_pll.dfs[0].dfs_ctrl.reg);
+		break;
+	case SYS_PLL_PFD1:
+	case SYS_PLL_PFD1_DIV2:
+		ctrl = readl(&ana_regs->sys_pll.dfs[1].dfs_ctrl.reg);
+		break;
+	case SYS_PLL_PFD2:
+	case SYS_PLL_PFD2_DIV2:
+		ctrl = readl(&ana_regs->sys_pll.dfs[2].dfs_ctrl.reg);
+		break;
+	case OSC_24M_CLK:
+		return 24000;
+	default:
+		printf("Invalid clock source to get rate\n");
+		return 0;
+	}
+
+	if (ctrl & PLL_CTRL_HW_CTRL_SEL) {
+		/* When using HW ctrl, check OSCPLL */
+		clk_on = ccm_clk_src_is_clk_on(source);
+		if (clk_on)
+			return decode_pll(source);
+		else
+			return 0;
+	} else {
+		/* controlled by pll registers */
+		return decode_pll(source);
+	}
+}
+
+u32 get_arm_core_clk(void)
+{
+	u32 val;
+
+	ccm_shared_gpr_get(SHARED_GPR_A55_CLK, &val);
+
+	if (val & SHARED_GPR_A55_CLK_SEL_PLL)
+		return decode_pll(ARM_PLL_CLK) * 1000;
+
+	return ccm_clk_root_get_rate(ARM_A55_CLK_ROOT);
+}
+
+unsigned int mxc_get_clock(enum mxc_clock clk)
+{
+	switch (clk) {
+	case MXC_ARM_CLK:
+		return get_arm_core_clk();
+	case MXC_IPG_CLK:
+		return ccm_clk_root_get_rate(BUS_WAKEUP_CLK_ROOT);
+	case MXC_CSPI_CLK:
+		return ccm_clk_root_get_rate(LPSPI1_CLK_ROOT);
+	case MXC_ESDHC_CLK:
+		return ccm_clk_root_get_rate(USDHC1_CLK_ROOT);
+	case MXC_ESDHC2_CLK:
+		return ccm_clk_root_get_rate(USDHC2_CLK_ROOT);
+	case MXC_ESDHC3_CLK:
+		return ccm_clk_root_get_rate(USDHC3_CLK_ROOT);
+	case MXC_UART_CLK:
+		return ccm_clk_root_get_rate(LPUART1_CLK_ROOT);
+	case MXC_FLEXSPI_CLK:
+		return ccm_clk_root_get_rate(FLEXSPI1_CLK_ROOT);
+	default:
+		return -1;
+	};
+
+	return -1;
+};
+
+int enable_i2c_clk(unsigned char enable, u32 i2c_num)
+{
+	if (i2c_num > 7)
+		return -EINVAL;
+
+	if (enable) {
+		/* 24M */
+		ccm_lpcg_on(CCGR_I2C1 + i2c_num, false);
+		ccm_clk_root_cfg(LPI2C1_CLK_ROOT + i2c_num, OSC_24M_CLK, 1);
+		ccm_lpcg_on(CCGR_I2C1 + i2c_num, true);
+	} else {
+		ccm_lpcg_on(CCGR_I2C1 + i2c_num, false);
+	}
+
+	return 0;
+}
+
+u32 imx_get_i2cclk(u32 i2c_num)
+{
+	if (i2c_num > 7)
+		return -EINVAL;
+
+	return ccm_clk_root_get_rate(LPUART1_CLK_ROOT + i2c_num);
+}
+
 u32 get_lpuart_clk(void)
 {
-	return 24000000;
+	return mxc_get_clock(MXC_UART_CLK);
 }
+
+void init_uart_clk(u32 index)
+{
+	switch (index) {
+	case LPUART1_CLK_ROOT:
+		/* 24M */
+		ccm_lpcg_on(CCGR_URT1, false);
+		ccm_clk_root_cfg(LPUART1_CLK_ROOT, OSC_24M_CLK, 1);
+		ccm_lpcg_on(CCGR_URT1, true);
+		break;
+	default:
+		break;
+	}
+}
+
+void init_clk_usdhc(u32 index)
+{
+	/* 400 Mhz */
+	switch (index) {
+	case 0:
+		ccm_lpcg_on(CCGR_USDHC1, 0);
+		ccm_clk_root_cfg(USDHC1_CLK_ROOT, SYS_PLL_PFD1, 2);
+		ccm_lpcg_on(CCGR_USDHC1, 1);
+		break;
+	case 1:
+		ccm_lpcg_on(CCGR_USDHC2, 0);
+		ccm_clk_root_cfg(USDHC2_CLK_ROOT, SYS_PLL_PFD1, 2);
+		ccm_lpcg_on(CCGR_USDHC2, 1);
+		break;
+	case 2:
+		ccm_lpcg_on(CCGR_USDHC3, 0);
+		ccm_clk_root_cfg(USDHC3_CLK_ROOT, SYS_PLL_PFD1, 2);
+		ccm_lpcg_on(CCGR_USDHC3, 1);
+		break;
+	default:
+		return;
+	};
+}
+
+void enable_usboh3_clk(unsigned char enable)
+{
+	if (enable) {
+		ccm_clk_root_cfg(HSIO_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+		ccm_lpcg_on(CCGR_USBC, 1);
+	} else {
+		ccm_lpcg_on(CCGR_USBC, 0);
+	}
+}
+
+int clock_init(void)
+{
+	int i;
+
+	/* Set A55 periphal to 333M */
+	ccm_clk_root_cfg(ARM_A55_PERIPH_CLK_ROOT, SYS_PLL_PFD0, 3);
+	/* Set A55 mtr bus to 133M */
+	ccm_clk_root_cfg(ARM_A55_MTR_BUS_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+
+	/* Sentinel to 200M */
+	ccm_clk_root_cfg(SENTINEL_CLK_ROOT, SYS_PLL_PFD1_DIV2, 2);
+	/* Bus_wakeup to 133M */
+	ccm_clk_root_cfg(BUS_WAKEUP_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+	/* Bus_AON to 133M */
+	ccm_clk_root_cfg(BUS_AON_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+	/* M33 to 200M */
+	ccm_clk_root_cfg(M33_CLK_ROOT, SYS_PLL_PFD1_DIV2, 2);
+	/*
+	 * WAKEUP_AXI to 312.5M, because of FEC only can support to 320M for
+	 * generating MII clock at 2.5M
+	 */
+	ccm_clk_root_cfg(WAKEUP_AXI_CLK_ROOT, SYS_PLL_PFD2, 2);
+	/* SWO TRACE to 133M */
+	ccm_clk_root_cfg(SWO_TRACE_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+	/* M33 systetick to 133M */
+	ccm_clk_root_cfg(M33_SYSTICK_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+	/* NIC to 400M */
+	ccm_clk_root_cfg(NIC_CLK_ROOT, SYS_PLL_PFD1, 2);
+	/* NIC_APB to 133M */
+	ccm_clk_root_cfg(NIC_APB_CLK_ROOT, SYS_PLL_PFD1_DIV2, 3);
+
+	/* allow for non-secure access */
+	for (i = 0; i < OSCPLL_END; i++)
+		ccm_clk_src_tz_access(i, true, false, false);
+
+	for (i = 0; i < CLK_ROOT_NUM; i++)
+		ccm_clk_root_tz_access(i, true, false, false);
+
+	for (i = 0; i < CCGR_NUM; i++)
+		ccm_lpcg_tz_access(i, true, false, false);
+
+	for (i = 0; i < SHARED_GPR_NUM; i++)
+		ccm_shared_gpr_tz_access(i, true, false, false);
+
+	return 0;
+}
+
+int set_clk_eqos(enum enet_freq type)
+{
+	u32 eqos_post_div;
+
+	switch (type) {
+	case ENET_125MHZ:
+		eqos_post_div = 2; /* 250M clock */
+		break;
+	case ENET_50MHZ:
+		eqos_post_div = 5; /* 100M clock */
+		break;
+	case ENET_25MHZ:
+		eqos_post_div = 10; /* 50M clock*/
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* disable the clock first */
+	ccm_lpcg_on(CCGR_ENETQOS, false);
+
+	ccm_clk_root_cfg(ENET_CLK_ROOT, SYS_PLL_PFD0_DIV2, eqos_post_div);
+	ccm_clk_root_cfg(ENET_TIMER2_CLK_ROOT, SYS_PLL_PFD0_DIV2, 5);
+
+	/* enable clock */
+	ccm_lpcg_on(CCGR_ENETQOS, true);
+
+	return 0;
+}
+
+u32 imx_get_eqos_csr_clk(void)
+{
+	return ccm_clk_root_get_rate(WAKEUP_AXI_CLK_ROOT);
+}
+
+u32 imx_get_fecclk(void)
+{
+	return ccm_clk_root_get_rate(WAKEUP_AXI_CLK_ROOT);
+}
+
+int set_clk_enet(enum enet_freq type)
+{
+	u32 div;
+
+	/* disable the clock first */
+	ccm_lpcg_on(CCGR_ENET1, false);
+
+	switch (type) {
+	case ENET_125MHZ:
+		div = 2; /* 250Mhz */
+		break;
+	case ENET_50MHZ:
+		div = 5; /* 100Mhz */
+		break;
+	case ENET_25MHZ:
+		div = 10; /* 50Mhz */
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ccm_clk_root_cfg(ENET_REF_CLK_ROOT, SYS_PLL_PFD0_DIV2, div);
+	ccm_clk_root_cfg(ENET_TIMER1_CLK_ROOT, SYS_PLL_PFD0_DIV2, 5);
+
+#ifdef CONFIG_FEC_MXC_25M_REF_CLK
+	ccm_clk_root_cfg(ENET_REF_PHY_CLK_ROOT, SYS_PLL_PFD0_DIV2, 20);
+#endif
+
+	/* enable clock */
+	ccm_lpcg_on(CCGR_ENET1, true);
+
+	return 0;
+}
+
+/*
+ * Dump some clockes.
+ */
+#ifndef CONFIG_SPL_BUILD
+int do_showclocks(struct cmd_tbl *cmdtp, int flag, int argc, char * const argv[])
+{
+	u32 freq;
+
+	freq = decode_pll(ARM_PLL_CLK);
+	printf("ARM_PLL    %8d MHz\n", freq / 1000);
+	freq = decode_pll(DRAM_PLL_CLK);
+	printf("DRAM_PLL    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD0);
+	printf("SYS_PLL_PFD0    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD0_DIV2);
+	printf("SYS_PLL_PFD0_DIV2    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD1);
+	printf("SYS_PLL_PFD1    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD1_DIV2);
+	printf("SYS_PLL_PFD1_DIV2    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD2);
+	printf("SYS_PLL_PFD2    %8d MHz\n", freq / 1000);
+	freq = decode_pll(SYS_PLL_PFD2_DIV2);
+	printf("SYS_PLL_PFD2_DIV2    %8d MHz\n", freq / 1000);
+	freq = mxc_get_clock(MXC_ARM_CLK);
+	printf("ARM CORE    %8d MHz\n", freq / 1000000);
+	freq = mxc_get_clock(MXC_IPG_CLK);
+	printf("IPG         %8d MHz\n", freq / 1000000);
+	freq = mxc_get_clock(MXC_UART_CLK);
+	printf("UART3          %8d MHz\n", freq / 1000000);
+	freq = mxc_get_clock(MXC_ESDHC_CLK);
+	printf("USDHC1         %8d MHz\n", freq / 1000000);
+	freq = mxc_get_clock(MXC_FLEXSPI_CLK);
+	printf("FLEXSPI           %8d MHz\n", freq / 1000000);
+
+	return 0;
+}
+
+U_BOOT_CMD(
+	clocks,	CONFIG_SYS_MAXARGS, 1, do_showclocks,
+	"display clocks",
+	""
+);
+#endif

arch/arm/mach-imx/imx9/clock_root.c

@@ -0,0 +1,438 @@
+// SPDX-License-Identifier: GPL-2.0+
+/*
+ * Copyright 2022 NXP
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ */
+
+#include <common.h>
+#include <command.h>
+#include <asm/arch/clock.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/arch/ccm_regs.h>
+#include <asm/global_data.h>
+#include <linux/iopoll.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+static struct ccm_reg *ccm_reg = (struct ccm_reg *)CCM_BASE_ADDR;
+
+static enum ccm_clk_src clk_root_mux[][4] = {
+	{ OSC_24M_CLK, SYS_PLL_PFD0, SYS_PLL_PFD1, SYS_PLL_PFD2 }, /* bus */
+	{ OSC_24M_CLK, SYS_PLL_PFD0_DIV2, SYS_PLL_PFD1_DIV2, SYS_PLL_PFD2_DIV2 }, /* non-IO */
+	{ OSC_24M_CLK, SYS_PLL_PFD0_DIV2, SYS_PLL_PFD1_DIV2, VIDEO_PLL_CLK }, /* IO*/
+	{ OSC_24M_CLK, SYS_PLL_PFD0, AUDIO_PLL_CLK, EXT_CLK  }, /* TPM */
+	{ OSC_24M_CLK, AUDIO_PLL_CLK, VIDEO_PLL_CLK, EXT_CLK }, /* Audio */
+	{ OSC_24M_CLK, AUDIO_PLL_CLK, VIDEO_PLL_CLK, SYS_PLL_PFD0 }, /* Video */
+	{ OSC_24M_CLK, SYS_PLL_PFD0, SYS_PLL_PFD1, AUDIO_PLL_CLK }, /* CKO1 */
+	{ OSC_24M_CLK, SYS_PLL_PFD0, SYS_PLL_PFD1, VIDEO_PLL_CLK }, /* CKO2 */
+	{ OSC_24M_CLK, AUDIO_PLL_CLK, VIDEO_PLL_CLK, SYS_PLL_PFD2 }, /* CAMSCAN */
+};
+
+static struct clk_root_map clk_root_array[] = {
+	{ ARM_A55_PERIPH_CLK_ROOT,	0 },
+	{ ARM_A55_MTR_BUS_CLK_ROOT,	2 },
+	{ ARM_A55_CLK_ROOT,		0 },
+	{ M33_CLK_ROOT,			2 },
+	{ SENTINEL_CLK_ROOT,		2 },
+	{ BUS_WAKEUP_CLK_ROOT,		2 },
+	{ BUS_AON_CLK_ROOT,		2 },
+	{ WAKEUP_AXI_CLK_ROOT,		0 },
+	{ SWO_TRACE_CLK_ROOT,		2 },
+	{ M33_SYSTICK_CLK_ROOT,		2 },
+	{ FLEXIO1_CLK_ROOT,		2 },
+	{ FLEXIO2_CLK_ROOT,		2 },
+	{ LPIT1_CLK_ROOT,		2 },
+	{ LPIT2_CLK_ROOT,		2 },
+	{ LPTMR1_CLK_ROOT,		2 },
+	{ LPTMR2_CLK_ROOT,		2 },
+	{ TPM1_CLK_ROOT,		3 },
+	{ TPM2_CLK_ROOT,		3 },
+	{ TPM3_CLK_ROOT,		3 },
+	{ TPM4_CLK_ROOT,		3 },
+	{ TPM5_CLK_ROOT,		3 },
+	{ TPM6_CLK_ROOT,		3 },
+	{ FLEXSPI1_CLK_ROOT,		0 },
+	{ CAN1_CLK_ROOT,		2 },
+	{ CAN2_CLK_ROOT,		2 },
+	{ LPUART1_CLK_ROOT,		2 },
+	{ LPUART2_CLK_ROOT,		2 },
+	{ LPUART3_CLK_ROOT,		2 },
+	{ LPUART4_CLK_ROOT,		2 },
+	{ LPUART5_CLK_ROOT,		2 },
+	{ LPUART6_CLK_ROOT,		2 },
+	{ LPUART7_CLK_ROOT,		2 },
+	{ LPUART8_CLK_ROOT,		2 },
+	{ LPI2C1_CLK_ROOT,		2 },
+	{ LPI2C2_CLK_ROOT,		2 },
+	{ LPI2C3_CLK_ROOT,		2 },
+	{ LPI2C4_CLK_ROOT,		2 },
+	{ LPI2C5_CLK_ROOT,		2 },
+	{ LPI2C6_CLK_ROOT,		2 },
+	{ LPI2C7_CLK_ROOT,		2 },
+	{ LPI2C8_CLK_ROOT,		2 },
+	{ LPSPI1_CLK_ROOT,		2 },
+	{ LPSPI2_CLK_ROOT,		2 },
+	{ LPSPI3_CLK_ROOT,		2 },
+	{ LPSPI4_CLK_ROOT,		2 },
+	{ LPSPI5_CLK_ROOT,		2 },
+	{ LPSPI6_CLK_ROOT,		2 },
+	{ LPSPI7_CLK_ROOT,		2 },
+	{ LPSPI8_CLK_ROOT,		2 },
+	{ I3C1_CLK_ROOT,		2 },
+	{ I3C2_CLK_ROOT,		2 },
+	{ USDHC1_CLK_ROOT,		0 },
+	{ USDHC2_CLK_ROOT,		0 },
+	{ USDHC3_CLK_ROOT,		0 },
+	{ SAI1_CLK_ROOT,		4 },
+	{ SAI2_CLK_ROOT,		4 },
+	{ SAI3_CLK_ROOT,		4 },
+	{ CCM_CKO1_CLK_ROOT,		6 },
+	{ CCM_CKO2_CLK_ROOT,		7 },
+	{ CCM_CKO3_CLK_ROOT,		6 },
+	{ CCM_CKO4_CLK_ROOT,		7 },
+	{ HSIO_CLK_ROOT,		2 },
+	{ HSIO_USB_TEST_60M_CLK_ROOT,	2 },
+	{ HSIO_ACSCAN_80M_CLK_ROOT,	2 },
+	{ HSIO_ACSCAN_480M_CLK_ROOT,	0 },
+	{ NIC_CLK_ROOT,			0 },
+	{ NIC_APB_CLK_ROOT,		2 },
+	{ ML_APB_CLK_ROOT,		2 },
+	{ ML_CLK_ROOT,			0 },
+	{ MEDIA_AXI_CLK_ROOT,		0 },
+	{ MEDIA_APB_CLK_ROOT,		2 },
+	{ MEDIA_LDB_CLK_ROOT,		5 },
+	{ MEDIA_DISP_PIX_CLK_ROOT,	5 },
+	{ CAM_PIX_CLK_ROOT,		5 },
+	{ MIPI_TEST_BYTE_CLK_ROOT,	5 },
+	{ MIPI_PHY_CFG_CLK_ROOT,	5 },
+	{ DRAM_ALT_CLK_ROOT,		0 },
+	{ DRAM_APB_CLK_ROOT,		1 },
+	{ ADC_CLK_ROOT,			2 },
+	{ PDM_CLK_ROOT,			4 },
+	{ TSTMR1_CLK_ROOT,		2 },
+	{ TSTMR2_CLK_ROOT,		2 },
+	{ MQS1_CLK_ROOT,		4 },
+	{ MQS2_CLK_ROOT,		4 },
+	{ AUDIO_XCVR_CLK_ROOT,		1 },
+	{ SPDIF_CLK_ROOT,		4 },
+	{ ENET_CLK_ROOT,		1 },
+	{ ENET_TIMER1_CLK_ROOT,		2 },
+	{ ENET_TIMER2_CLK_ROOT,		2 },
+	{ ENET_REF_CLK_ROOT,		1 },
+	{ ENET_REF_PHY_CLK_ROOT,	2 },
+	{ I3C1_SLOW_CLK_ROOT,		2 },
+	{ I3C2_SLOW_CLK_ROOT,		2 },
+	{ USB_PHY_BURUNIN_CLK_ROOT,	2 },
+	{ PAL_CAME_SCAN_CLK_ROOT,	8 },
+};
+
+int ccm_clk_src_on(enum ccm_clk_src oscpll, bool enable)
+{
+	u32 authen;
+
+	if (oscpll >= OSCPLL_END)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_oscplls[oscpll].authen);
+
+	/* If using cpulpm, need disable it first */
+	if (authen & CCM_AUTHEN_CPULPM_MODE)
+		return -EPERM;
+
+	if (enable)
+		writel(1, &ccm_reg->clk_oscplls[oscpll].direct);
+	else
+		writel(0, &ccm_reg->clk_oscplls[oscpll].direct);
+
+	return 0;
+}
+
+/* auto mode, enable =  DIRECT[ON] | STATUS0[IN_USE] */
+int ccm_clk_src_auto(enum ccm_clk_src oscpll, bool enable)
+{
+	u32 authen;
+
+	if (oscpll >= OSCPLL_END)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_oscplls[oscpll].authen);
+
+	/* AUTO CTRL and CPULPM are mutual exclusion, need disable CPULPM first */
+	if (authen & CCM_AUTHEN_CPULPM_MODE)
+		return -EPERM;
+
+	if (enable)
+		writel(authen | CCM_AUTHEN_AUTO_CTRL, &ccm_reg->clk_oscplls[oscpll].authen);
+	else
+		writel((authen & ~CCM_AUTHEN_AUTO_CTRL), &ccm_reg->clk_oscplls[oscpll].authen);
+
+	return 0;
+}
+
+int ccm_clk_src_lpm(enum ccm_clk_src oscpll, bool enable)
+{
+	u32 authen;
+
+	if (oscpll >= OSCPLL_END)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_oscplls[oscpll].authen);
+
+	/* AUTO CTRL and CPULPM are mutual exclusion, need disable AUTO CTRL first */
+	if (authen & CCM_AUTHEN_AUTO_CTRL)
+		return -EPERM;
+
+	if (enable)
+		writel(authen | CCM_AUTHEN_CPULPM_MODE, &ccm_reg->clk_oscplls[oscpll].authen);
+	else
+		writel((authen & ~CCM_AUTHEN_CPULPM_MODE), &ccm_reg->clk_oscplls[oscpll].authen);
+
+	return 0;
+}
+
+int ccm_clk_src_config_lpm(enum ccm_clk_src oscpll, u32 domain, u32 lpm_val)
+{
+	u32 lpm, authen;
+
+	if (oscpll >= OSCPLL_END || domain >= 16)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_oscplls[oscpll].authen);
+	if (!(authen & CCM_AUTHEN_CPULPM_MODE))
+		return -EPERM;
+
+	if (domain > 7) {
+		lpm = readl(&ccm_reg->clk_oscplls[oscpll].lpm1);
+		lpm &= ~(0x3 << ((domain - 8) * 4));
+		lpm |= (lpm_val & 0x3) << ((domain - 8) * 4);
+		writel(lpm, &ccm_reg->clk_oscplls[oscpll].lpm1);
+	} else {
+		lpm = readl(&ccm_reg->clk_oscplls[oscpll].lpm0);
+		lpm &= ~(0x3 << (domain * 4));
+		lpm |= (lpm_val & 0x3) << (domain * 4);
+		writel(lpm, &ccm_reg->clk_oscplls[oscpll].lpm0);
+	}
+
+	return 0;
+}
+
+bool ccm_clk_src_is_clk_on(enum ccm_clk_src oscpll)
+{
+	return !!(readl(&ccm_reg->clk_oscplls[oscpll].status0) & 0x1);
+}
+
+int ccm_clk_src_tz_access(enum ccm_clk_src oscpll, bool non_secure, bool user_mode, bool lock_tz)
+{
+	u32 authen;
+
+	if (oscpll >= OSCPLL_END)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_oscplls[oscpll].authen);
+
+	authen |= non_secure ? CCM_AUTHEN_TZ_NS : 0;
+	authen |= user_mode ? CCM_AUTHEN_TZ_USER : 0;
+	authen |= lock_tz ? CCM_AUTHEN_LOCK_TZ : 0;
+
+	writel(authen, &ccm_reg->clk_oscplls[oscpll].authen);
+
+	return 0;
+}
+
+int ccm_clk_root_cfg(u32 clk_root_id, enum ccm_clk_src src, u32 div)
+{
+	int i;
+	int ret;
+	u32 mux, status;
+
+	if (clk_root_id >= CLK_ROOT_NUM || div > 256 || div == 0)
+		return -EINVAL;
+
+	mux = clk_root_array[clk_root_id].mux_type;
+
+	for (i = 0; i < 4; i++) {
+		if (src == clk_root_mux[mux][i])
+			break;
+	}
+
+	if (i == 4) {
+		printf("Invalid source [%u] for this clk root\n", src);
+		return -EINVAL;
+	}
+
+	writel((i << 8) | (div - 1), &ccm_reg->clk_roots[clk_root_id].control);
+
+	ret = readl_poll_timeout(&ccm_reg->clk_roots[clk_root_id].status0, status,
+				 !(status & CLK_ROOT_STATUS_CHANGING), 200000);
+	if (ret)
+		printf("%s: failed, status: 0x%x\n", __func__,
+		       readl(&ccm_reg->clk_roots[clk_root_id].status0));
+
+	return ret;
+};
+
+u32 ccm_clk_root_get_rate(u32 clk_root_id)
+{
+	u32 mux, status, div, rate;
+	enum ccm_clk_src src;
+
+	if (clk_root_id >= CLK_ROOT_NUM)
+		return 0;
+
+	status = readl(&ccm_reg->clk_roots[clk_root_id].control);
+
+	if (status & CLK_ROOT_STATUS_OFF)
+		return 0; /* clock is off */
+
+	mux = (status & CLK_ROOT_MUX_MASK) >> CLK_ROOT_MUX_SHIFT;
+	div = status & CLK_ROOT_DIV_MASK;
+	src = clk_root_mux[clk_root_array[clk_root_id].mux_type][mux];
+
+	rate = get_clk_src_rate(src) * 1000;
+
+	return rate / (div + 1); /* return in hz */
+}
+
+int ccm_clk_root_tz_access(u32 clk_root_id, bool non_secure, bool user_mode, bool lock_tz)
+{
+	u32 authen;
+
+	if (clk_root_id >= CLK_ROOT_NUM)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_roots[clk_root_id].authen);
+
+	authen |= non_secure ? CCM_AUTHEN_TZ_NS : 0;
+	authen |= user_mode ? CCM_AUTHEN_TZ_USER : 0;
+	authen |= lock_tz ? CCM_AUTHEN_LOCK_TZ : 0;
+
+	writel(authen, &ccm_reg->clk_roots[clk_root_id].authen);
+
+	return 0;
+}
+
+int ccm_lpcg_on(u32 lpcg, bool enable)
+{
+	u32 authen;
+
+	if (lpcg >= CCGR_NUM)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_lpcgs[lpcg].authen);
+
+	/* If using cpulpm, need disable it first */
+	if (authen & CCM_AUTHEN_CPULPM_MODE)
+		return -EPERM;
+
+	if (enable)
+		writel(1, &ccm_reg->clk_lpcgs[lpcg].direct);
+	else
+		writel(0, &ccm_reg->clk_lpcgs[lpcg].direct);
+
+	return 0;
+}
+
+int ccm_lpcg_lpm(u32 lpcg, bool enable)
+{
+	u32 authen;
+
+	if (lpcg >= CCGR_NUM)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_lpcgs[lpcg].authen);
+
+	if (enable)
+		writel(authen | CCM_AUTHEN_CPULPM_MODE, &ccm_reg->clk_lpcgs[lpcg].authen);
+	else
+		writel((authen & ~CCM_AUTHEN_CPULPM_MODE), &ccm_reg->clk_lpcgs[lpcg].authen);
+
+	return 0;
+}
+
+int ccm_lpcg_config_lpm(u32 lpcg, u32 domain, u32 lpm_val)
+{
+	u32 lpm, authen;
+
+	if (lpcg >= CCGR_NUM || domain >= 16)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_lpcgs[lpcg].authen);
+	if (!(authen & CCM_AUTHEN_CPULPM_MODE))
+		return -EPERM;
+
+	if (domain > 7) {
+		lpm = readl(&ccm_reg->clk_lpcgs[lpcg].lpm1);
+		lpm &= ~(0x3 << ((domain - 8) * 4));
+		lpm |= (lpm_val & 0x3) << ((domain - 8) * 4);
+		writel(lpm, &ccm_reg->clk_lpcgs[lpcg].lpm1);
+	} else {
+		lpm = readl(&ccm_reg->clk_lpcgs[lpcg].lpm0);
+		lpm &= ~(0x3 << (domain * 4));
+		lpm |= (lpm_val & 0x3) << (domain * 4);
+		writel(lpm, &ccm_reg->clk_lpcgs[lpcg].lpm0);
+	}
+
+	return 0;
+}
+
+bool ccm_lpcg_is_clk_on(u32 lpcg)
+{
+	return !!(readl(&ccm_reg->clk_lpcgs[lpcg].status0) & 0x1);
+}
+
+int ccm_lpcg_tz_access(u32 lpcg, bool non_secure, bool user_mode, bool lock_tz)
+{
+	u32 authen;
+
+	if (lpcg >= CCGR_NUM)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_lpcgs[lpcg].authen);
+
+	authen |= non_secure ? CCM_AUTHEN_TZ_NS : 0;
+	authen |= user_mode ? CCM_AUTHEN_TZ_USER : 0;
+	authen |= lock_tz ? CCM_AUTHEN_LOCK_TZ : 0;
+
+	writel(authen, &ccm_reg->clk_lpcgs[lpcg].authen);
+
+	return 0;
+}
+
+int ccm_shared_gpr_set(u32 gpr, u32 val)
+{
+	if (gpr >= SHARED_GPR_NUM)
+		return -EINVAL;
+
+	writel(val, &ccm_reg->clk_shared_gpr[gpr].gpr);
+
+	return 0;
+}
+
+int ccm_shared_gpr_get(u32 gpr, u32 *val)
+{
+	if (gpr >= SHARED_GPR_NUM || !val)
+		return -EINVAL;
+
+	*val = readl(&ccm_reg->clk_shared_gpr[gpr].gpr);
+
+	return 0;
+}
+
+int ccm_shared_gpr_tz_access(u32 gpr, bool non_secure, bool user_mode, bool lock_tz)
+{
+	u32 authen;
+
+	if (gpr >= SHARED_GPR_NUM)
+		return -EINVAL;
+
+	authen = readl(&ccm_reg->clk_shared_gpr[gpr].authen);
+
+	authen |= non_secure ? CCM_AUTHEN_TZ_NS : 0;
+	authen |= user_mode ? CCM_AUTHEN_TZ_USER : 0;
+	authen |= lock_tz ? CCM_AUTHEN_LOCK_TZ : 0;
+
+	writel(authen, &ccm_reg->clk_shared_gpr[gpr].authen);
+
+	return 0;
+}

arch/arm/mach-imx/imx9/soc.c

@@ -123,5 +123,8 @@ int ft_system_setup(void *blob, struct bd_info *bd)
 
 int arch_cpu_init(void)
 {
+	if (IS_ENABLED(CONFIG_SPL_BUILD))
+		clock_init();
+
 	return 0;
 }