diff --git a/drivers/mfd/rkx110_x120/hal/cru_rkx110.c b/drivers/mfd/rkx110_x120/hal/cru_rkx110.c index 5d53ec33dd60..4e1533fe1b31 100644 --- a/drivers/mfd/rkx110_x120/hal/cru_rkx110.c +++ b/drivers/mfd/rkx110_x120/hal/cru_rkx110.c @@ -263,6 +263,7 @@ static HAL_Status RKX11x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName uint32_t pll; uint8_t overMax = 0; HAL_Status ret = HAL_OK; + int i; if (clockName == RKX110_CLK_D_DSI_0_PATTERN_GEN || clockName == RKX110_CLK_D_DSI_1_PATTERN_GEN) { @@ -326,12 +327,21 @@ static HAL_Status RKX11x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName /* PLL change closest new rate <= 1200M if need */ if (!pRate) { - pRate = (_MHZ(1200) / rate) * rate; - } - - ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); - if (ret != HAL_OK) { - return ret; + if (!rate || rate > _MHZ(1200)) + return HAL_ERROR; + for (i = _MHZ(1200) / rate; i > _MHZ(24) / rate; i--) { + pRate = i * rate; + ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret == HAL_OK) + break; + } + if (ret != HAL_OK) + return ret; + } else { + ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret != HAL_OK) { + return ret; + } } /* if success, continue to set divider */ diff --git a/drivers/mfd/rkx110_x120/hal/cru_rkx111.c b/drivers/mfd/rkx110_x120/hal/cru_rkx111.c index 41da74fb027b..1f22f406ffce 100644 --- a/drivers/mfd/rkx110_x120/hal/cru_rkx111.c +++ b/drivers/mfd/rkx110_x120/hal/cru_rkx111.c @@ -304,6 +304,7 @@ static HAL_Status RKX11x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName uint32_t pll; uint8_t overMax = 0; HAL_Status ret = HAL_OK; + int i; if (clockName == RKX110_CLK_D_DSI_0_PATTERN_GEN) { clockName = RKX111_CPS_DCLK_D_DSI_0_REC; @@ -371,12 +372,21 @@ static HAL_Status RKX11x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName /* PLL change closest new rate <= 1200M if need */ if (!pRate) { - pRate = (_MHZ(1200) / rate) * rate; - } - - ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); - if (ret != HAL_OK) { - return ret; + if (!rate || rate > _MHZ(1200)) + return HAL_ERROR; + for (i = _MHZ(1200) / rate; i > _MHZ(24) / rate; i--) { + pRate = i * rate; + ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret == HAL_OK) + break; + } + if (ret != HAL_OK) + return ret; + } else { + ret = RKX11x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret != HAL_OK) { + return ret; + } } /* if success, continue to set divider */ diff --git a/drivers/mfd/rkx110_x120/hal/cru_rkx120.c b/drivers/mfd/rkx110_x120/hal/cru_rkx120.c index 07da30ba0fd4..025a913626f8 100644 --- a/drivers/mfd/rkx110_x120/hal/cru_rkx120.c +++ b/drivers/mfd/rkx110_x120/hal/cru_rkx120.c @@ -252,6 +252,7 @@ static HAL_Status RKX12x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName uint32_t pll; uint8_t overMax; HAL_Status ret = HAL_OK; + int i; switch (clockName) { case RKX120_CPS_PLL_TXPLL: @@ -298,12 +299,21 @@ static HAL_Status RKX12x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName /* PLL change closest new rate <= 1200M if need */ if (!pRate) { - pRate = (_MHZ(1200) / rate) * rate; - } - - ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); - if (ret != HAL_OK) { - return ret; + if (!rate || rate > _MHZ(1200)) + return HAL_ERROR; + for (i = _MHZ(1200) / rate; i > _MHZ(24) / rate; i--) { + pRate = i * rate; + ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret == HAL_OK) + break; + } + if (ret != HAL_OK) + return ret; + } else { + ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret != HAL_OK) { + return ret; + } } /* if success, continue to set divider */ diff --git a/drivers/mfd/rkx110_x120/hal/cru_rkx121.c b/drivers/mfd/rkx110_x120/hal/cru_rkx121.c index 350ce6c47f48..dfa1c62a30bd 100644 --- a/drivers/mfd/rkx110_x120/hal/cru_rkx121.c +++ b/drivers/mfd/rkx110_x120/hal/cru_rkx121.c @@ -263,6 +263,7 @@ static HAL_Status RKX12x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName uint32_t pll; uint8_t overMax; HAL_Status ret = HAL_OK; + int i; switch (clockName) { case RKX120_CPS_PLL_TXPLL: @@ -309,12 +310,21 @@ static HAL_Status RKX12x_HAL_CRU_ClkSetFreq(struct hwclk *hw, uint32_t clockName /* PLL change closest new rate <= 1200M if need */ if (!pRate) { - pRate = (_MHZ(1200) / rate) * rate; - } - - ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); - if (ret != HAL_OK) { - return ret; + if (!rate || rate > _MHZ(1200)) + return HAL_ERROR; + for (i = _MHZ(1200) / rate; i > _MHZ(24) / rate; i--) { + pRate = i * rate; + ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret == HAL_OK) + break; + } + if (ret != HAL_OK) + return ret; + } else { + ret = RKX12x_HAL_CRU_ClkSetFreq(hw, pll, pRate); + if (ret != HAL_OK) { + return ret; + } } /* if success, continue to set divider */