diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c index 536889c..de849ee 100644 --- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.c +++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.c @@ -2559,74 +2559,6 @@ } /** - * pll_get_lockbit() - Returns lockbit index for pll id - * @pll_id: Id of the pll - * - * This function return the PLL_LOCKED bit index in - * pll status register accosiated with given pll id. - * - * Return: Returns bit index - */ -static int pll_get_lockbit(unsigned int pll_id) -{ - switch (pll_id) { - case CLK_APLL_INT: - case CLK_IOPLL_INT: - return 0; - case CLK_DPLL_INT: - case CLK_RPLL_INT: - return 1; - case CLK_VPLL_INT: - return 2; - default: - return -1; - } -} - -/** - * pm_api_pll_bypass_and_reset() - Bypass and reset PLL - * @clock_id: Id of the PLL - * - * This function is to bypass and reset PLL. - */ -static inline enum pm_ret_status -pm_api_pll_bypass_and_reset(unsigned int clock_id, unsigned int flag) -{ - enum pm_ret_status ret = PM_RET_SUCCESS; - unsigned int reg, val; - int lockbit; - - reg = clocks[clock_id].control_reg; - - if (flag & CLK_PLL_RESET_ASSERT) { - ret = pm_mmio_write(reg, PLLCTRL_BP_MASK, PLLCTRL_BP_MASK); - if (ret != PM_RET_SUCCESS) - return ret; - ret = pm_mmio_write(reg, PLLCTRL_RESET_MASK, - PLLCTRL_RESET_MASK); - if (ret != PM_RET_SUCCESS) - return ret; - } - if (flag & CLK_PLL_RESET_RELEASE) { - ret = pm_mmio_write(reg, PLLCTRL_RESET_MASK, - ~PLLCTRL_RESET_MASK); - if (ret != PM_RET_SUCCESS) - return ret; - - lockbit = pll_get_lockbit(clock_id); - do { - ret = pm_mmio_read(clocks[clock_id].status_reg, &val); - if (ret != PM_RET_SUCCESS) - return ret; - } while ((lockbit >= 0) && !(val & (1 << lockbit))); - - ret = pm_mmio_write(reg, PLLCTRL_BP_MASK, - ~(unsigned int)PLLCTRL_BP_MASK); - } - return ret; -} - -/** * pm_clock_pll_enable() - "Enable" the PLL clock (lock the PLL) * @pll: PLL to be locked * @@ -2696,95 +2628,6 @@ return PM_RET_SUCCESS; } -static enum pm_ret_status pm_api_clk_set_divider(unsigned int clock_id, - uint32_t divider) -{ - enum pm_ret_status ret = PM_RET_SUCCESS; - struct pm_clock_node *nodes; - uint8_t num_nodes; - uint16_t div1, div2; - unsigned int reg, mask = 0, val = 0, i; - uint8_t div1_width = NA_WIDTH, div1_offset = NA_SHIFT; - uint8_t div2_width = NA_WIDTH, div2_offset = NA_SHIFT; - - div1 = (uint16_t)(divider & 0xFFFFU); - div2 = (uint16_t)((divider >> 16) & 0xFFFFU); - - reg = clocks[clock_id].control_reg; - - nodes = *clocks[clock_id].nodes; - num_nodes = clocks[clock_id].num_nodes; - for (i = 0; i < num_nodes; i++) { - if (nodes->type == TYPE_DIV1) { - div1_offset = nodes->offset; - div1_width = nodes->width; - } - if (nodes->type == TYPE_DIV2) { - div2_offset = nodes->offset; - div2_width = nodes->width; - } - nodes++; - } - - if (div1 != (uint16_t)-1) { - if (div1_width == NA_WIDTH) - return PM_RET_ERROR_NOTSUPPORTED; - val |= div1 << div1_offset; - mask |= BIT_MASK(div1_offset, div1_width); - } - if (div2 != (uint16_t)-1) { - if (div2_width == NA_WIDTH) - return PM_RET_ERROR_NOTSUPPORTED; - val |= div2 << div2_offset; - mask |= BIT_MASK(div2_offset, div2_width); - } - ret = pm_mmio_write(reg, mask, val); - - return ret; -} - -static enum pm_ret_status pm_api_pll_set_divider(unsigned int clock_id, - unsigned int divider) -{ - unsigned int reg = clocks[clock_id].control_reg; - enum pm_ret_status ret; - - pm_api_pll_bypass_and_reset(clock_id, CLK_PLL_RESET_ASSERT); - ret = pm_mmio_write(reg, PLL_FBDIV_MASK, divider << PLL_FBDIV_SHIFT); - pm_api_pll_bypass_and_reset(clock_id, CLK_PLL_RESET_RELEASE); - - return ret; -} - -/** - * pm_api_clock_setdivider - Set the clock divider for given id - * @clock_id Id of the clock - * @divider Divider value - * - * This function is used by master to set divider for any clock - * to achieve desired rate. - * - * Return: Returns status, either success or error+reason. - */ -enum pm_ret_status pm_api_clock_setdivider(unsigned int clock_id, - unsigned int divider) -{ - enum pm_ret_status ret; - - if (!pm_clock_valid(clock_id)) - return PM_RET_ERROR_ARGS; - - if (pm_clock_type(clock_id) != CLK_TYPE_OUTPUT) - return PM_RET_ERROR_NOTSUPPORTED; - - if (ISPLL(clock_id)) - ret = pm_api_pll_set_divider(clock_id, divider); - else - ret = pm_api_clk_set_divider(clock_id, divider); - - return ret; -} - static enum pm_ret_status pm_api_clk_get_divider(unsigned int clock_id, uint32_t *divider) { diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h index dfdaf89..543f2ad 100644 --- a/plat/xilinx/zynqmp/pm_service/pm_api_clock.h +++ b/plat/xilinx/zynqmp/pm_service/pm_api_clock.h @@ -299,9 +299,6 @@ enum pm_ret_status pm_clock_pll_disable(struct pm_pll *pll); enum pm_ret_status pm_clock_pll_get_state(struct pm_pll *pll, unsigned int *state); - -enum pm_ret_status pm_api_clock_setdivider(unsigned int clock_id, - unsigned int divider); enum pm_ret_status pm_api_clock_getdivider(unsigned int clock_id, unsigned int *divider); enum pm_ret_status pm_api_clock_setrate(unsigned int clock_id, diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c index 49471dc..91a1461 100644 --- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.c +++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.c @@ -964,7 +964,37 @@ enum pm_ret_status pm_clock_setdivider(unsigned int clock_id, unsigned int divider) { - return pm_api_clock_setdivider(clock_id, divider); + enum pm_ret_status status; + enum pm_node_id nid; + enum pm_clock_div_id div_id; + uint32_t payload[PAYLOAD_ARG_CNT]; + const uint32_t div0 = 0xFFFF0000; + const uint32_t div1 = 0x0000FFFF; + uint32_t val; + + /* Get PLL node ID using PLL clock ID */ + status = pm_clock_get_pll_node_id(clock_id, &nid); + if (status == PM_RET_SUCCESS) + return pm_pll_set_parameter(nid, PM_PLL_PARAM_FBDIV, divider); + + /* Check if clock ID is a valid on-chip clock */ + status = pm_clock_id_is_valid(clock_id); + if (status != PM_RET_SUCCESS) + return status; + + if (div0 == (divider & div0)) { + div_id = PM_CLOCK_DIV0_ID; + val = divider & ~div0; + } else if (div1 == (divider & div1)) { + div_id = PM_CLOCK_DIV1_ID; + val = (divider & ~div1) >> 16; + } else { + return PM_RET_ERROR_ARGS; + } + + /* Send request to the PMU */ + PM_PACK_PAYLOAD4(payload, PM_CLOCK_SETDIVIDER, clock_id, div_id, val); + return pm_ipi_send_sync(primary_proc, payload, NULL, 0); } /** diff --git a/plat/xilinx/zynqmp/pm_service/pm_defs.h b/plat/xilinx/zynqmp/pm_service/pm_defs.h index 7c23893..cae36c9 100644 --- a/plat/xilinx/zynqmp/pm_service/pm_defs.h +++ b/plat/xilinx/zynqmp/pm_service/pm_defs.h @@ -308,4 +308,13 @@ PM_PLL_MODE_MAX, }; +/** + * @PM_CLOCK_DIV0_ID: Clock divider 0 + * @PM_CLOCK_DIV1_ID: Clock divider 1 + */ +enum pm_clock_div_id { + PM_CLOCK_DIV0_ID, + PM_CLOCK_DIV1_ID, +}; + #endif /* PM_DEFS_H */