@@ -122,7 +122,9 @@ static const struct {
122122};
123123
124124static void ufs_qcom_get_default_testbus_cfg (struct ufs_qcom_host * host );
125- static int ufs_qcom_set_core_clk_ctrl (struct ufs_hba * hba , unsigned long freq );
125+ static unsigned long ufs_qcom_opp_freq_to_clk_freq (struct ufs_hba * hba ,
126+ unsigned long freq , char * name );
127+ static int ufs_qcom_set_core_clk_ctrl (struct ufs_hba * hba , bool is_scale_up , unsigned long freq );
126128
127129static struct ufs_qcom_host * rcdev_to_ufs_host (struct reset_controller_dev * rcd )
128130{
@@ -506,10 +508,9 @@ static int ufs_qcom_power_up_sequence(struct ufs_hba *hba)
506508 if (ret )
507509 return ret ;
508510
509- if (phy -> power_count ) {
511+ if (phy -> power_count )
510512 phy_power_off (phy );
511- phy_exit (phy );
512- }
513+
513514
514515 /* phy initialization - calibrate the phy */
515516 ret = phy_init (phy );
@@ -597,13 +598,14 @@ static int ufs_qcom_hce_enable_notify(struct ufs_hba *hba,
597598 *
598599 * @hba: host controller instance
599600 * @is_pre_scale_up: flag to check if pre scale up condition.
601+ * @freq: target opp freq
600602 * Return: zero for success and non-zero in case of a failure.
601603 */
602- static int ufs_qcom_cfg_timers (struct ufs_hba * hba , bool is_pre_scale_up )
604+ static int ufs_qcom_cfg_timers (struct ufs_hba * hba , bool is_pre_scale_up , unsigned long freq )
603605{
604606 struct ufs_qcom_host * host = ufshcd_get_variant (hba );
605607 struct ufs_clk_info * clki ;
606- unsigned long core_clk_rate = 0 ;
608+ unsigned long clk_freq = 0 ;
607609 u32 core_clk_cycles_per_us ;
608610
609611 /*
@@ -615,22 +617,34 @@ static int ufs_qcom_cfg_timers(struct ufs_hba *hba, bool is_pre_scale_up)
615617 if (host -> hw_ver .major < 4 && !ufshcd_is_intr_aggr_allowed (hba ))
616618 return 0 ;
617619
620+ if (hba -> use_pm_opp && freq != ULONG_MAX ) {
621+ clk_freq = ufs_qcom_opp_freq_to_clk_freq (hba , freq , "core_clk" );
622+ if (clk_freq )
623+ goto cfg_timers ;
624+ }
625+
618626 list_for_each_entry (clki , & hba -> clk_list_head , list ) {
619627 if (!strcmp (clki -> name , "core_clk" )) {
628+ if (freq == ULONG_MAX ) {
629+ clk_freq = clki -> max_freq ;
630+ break ;
631+ }
632+
620633 if (is_pre_scale_up )
621- core_clk_rate = clki -> max_freq ;
634+ clk_freq = clki -> max_freq ;
622635 else
623- core_clk_rate = clk_get_rate (clki -> clk );
636+ clk_freq = clk_get_rate (clki -> clk );
624637 break ;
625638 }
626639
627640 }
628641
642+ cfg_timers :
629643 /* If frequency is smaller than 1MHz, set to 1MHz */
630- if (core_clk_rate < DEFAULT_CLK_RATE_HZ )
631- core_clk_rate = DEFAULT_CLK_RATE_HZ ;
644+ if (clk_freq < DEFAULT_CLK_RATE_HZ )
645+ clk_freq = DEFAULT_CLK_RATE_HZ ;
632646
633- core_clk_cycles_per_us = core_clk_rate / USEC_PER_SEC ;
647+ core_clk_cycles_per_us = clk_freq / USEC_PER_SEC ;
634648 if (ufshcd_readl (hba , REG_UFS_SYS1CLK_1US ) != core_clk_cycles_per_us ) {
635649 ufshcd_writel (hba , core_clk_cycles_per_us , REG_UFS_SYS1CLK_1US );
636650 /*
@@ -650,13 +664,13 @@ static int ufs_qcom_link_startup_notify(struct ufs_hba *hba,
650664
651665 switch (status ) {
652666 case PRE_CHANGE :
653- if (ufs_qcom_cfg_timers (hba , false)) {
667+ if (ufs_qcom_cfg_timers (hba , false, ULONG_MAX )) {
654668 dev_err (hba -> dev , "%s: ufs_qcom_cfg_timers() failed\n" ,
655669 __func__ );
656670 return - EINVAL ;
657671 }
658672
659- err = ufs_qcom_set_core_clk_ctrl (hba , ULONG_MAX );
673+ err = ufs_qcom_set_core_clk_ctrl (hba , true, ULONG_MAX );
660674 if (err )
661675 dev_err (hba -> dev , "cfg core clk ctrl failed\n" );
662676 /*
@@ -928,17 +942,6 @@ static int ufs_qcom_pwr_change_notify(struct ufs_hba *hba,
928942
929943 break ;
930944 case POST_CHANGE :
931- if (ufs_qcom_cfg_timers (hba , false)) {
932- dev_err (hba -> dev , "%s: ufs_qcom_cfg_timers() failed\n" ,
933- __func__ );
934- /*
935- * we return error code at the end of the routine,
936- * but continue to configure UFS_PHY_TX_LANE_ENABLE
937- * and bus voting as usual
938- */
939- ret = - EINVAL ;
940- }
941-
942945 /* cache the power mode parameters to use internally */
943946 memcpy (& host -> dev_req_params ,
944947 dev_req_params , sizeof (* dev_req_params ));
@@ -1414,29 +1417,46 @@ static int ufs_qcom_set_clk_40ns_cycles(struct ufs_hba *hba,
14141417 return ufshcd_dme_set (hba , UIC_ARG_MIB (PA_VS_CORE_CLK_40NS_CYCLES ), reg );
14151418}
14161419
1417- static int ufs_qcom_set_core_clk_ctrl (struct ufs_hba * hba , unsigned long freq )
1420+ static int ufs_qcom_set_core_clk_ctrl (struct ufs_hba * hba , bool is_scale_up , unsigned long freq )
14181421{
14191422 struct ufs_qcom_host * host = ufshcd_get_variant (hba );
14201423 struct list_head * head = & hba -> clk_list_head ;
14211424 struct ufs_clk_info * clki ;
14221425 u32 cycles_in_1us = 0 ;
14231426 u32 core_clk_ctrl_reg ;
1427+ unsigned long clk_freq ;
14241428 int err ;
14251429
1430+ if (hba -> use_pm_opp && freq != ULONG_MAX ) {
1431+ clk_freq = ufs_qcom_opp_freq_to_clk_freq (hba , freq , "core_clk_unipro" );
1432+ if (clk_freq ) {
1433+ cycles_in_1us = ceil (clk_freq , HZ_PER_MHZ );
1434+ goto set_core_clk_ctrl ;
1435+ }
1436+ }
1437+
14261438 list_for_each_entry (clki , head , list ) {
14271439 if (!IS_ERR_OR_NULL (clki -> clk ) &&
14281440 !strcmp (clki -> name , "core_clk_unipro" )) {
1429- if (!clki -> max_freq )
1441+ if (!clki -> max_freq ) {
14301442 cycles_in_1us = 150 ; /* default for backwards compatibility */
1431- else if (freq == ULONG_MAX )
1443+ break ;
1444+ }
1445+
1446+ if (freq == ULONG_MAX ) {
14321447 cycles_in_1us = ceil (clki -> max_freq , HZ_PER_MHZ );
1433- else
1434- cycles_in_1us = ceil ( freq , HZ_PER_MHZ );
1448+ break ;
1449+ }
14351450
1451+ if (is_scale_up )
1452+ cycles_in_1us = ceil (clki -> max_freq , HZ_PER_MHZ );
1453+ else
1454+ cycles_in_1us = ceil (clk_get_rate (clki -> clk ), HZ_PER_MHZ );
14361455 break ;
14371456 }
14381457 }
14391458
1459+ set_core_clk_ctrl :
14401460 err = ufshcd_dme_get (hba ,
14411461 UIC_ARG_MIB (DME_VS_CORE_CLK_CTRL ),
14421462 & core_clk_ctrl_reg );
@@ -1473,13 +1493,13 @@ static int ufs_qcom_clk_scale_up_pre_change(struct ufs_hba *hba, unsigned long f
14731493{
14741494 int ret ;
14751495
1476- ret = ufs_qcom_cfg_timers (hba , true);
1496+ ret = ufs_qcom_cfg_timers (hba , true, freq );
14771497 if (ret ) {
14781498 dev_err (hba -> dev , "%s ufs cfg timer failed\n" , __func__ );
14791499 return ret ;
14801500 }
14811501 /* set unipro core clock attributes and clear clock divider */
1482- return ufs_qcom_set_core_clk_ctrl (hba , freq );
1502+ return ufs_qcom_set_core_clk_ctrl (hba , true, freq );
14831503}
14841504
14851505static int ufs_qcom_clk_scale_up_post_change (struct ufs_hba * hba )
@@ -1510,8 +1530,15 @@ static int ufs_qcom_clk_scale_down_pre_change(struct ufs_hba *hba)
15101530
15111531static int ufs_qcom_clk_scale_down_post_change (struct ufs_hba * hba , unsigned long freq )
15121532{
1533+ int ret ;
1534+
1535+ ret = ufs_qcom_cfg_timers (hba , false, freq );
1536+ if (ret ) {
1537+ dev_err (hba -> dev , "%s: ufs_qcom_cfg_timers() failed\n" , __func__ );
1538+ return ret ;
1539+ }
15131540 /* set unipro core clock attributes and clear clock divider */
1514- return ufs_qcom_set_core_clk_ctrl (hba , freq );
1541+ return ufs_qcom_set_core_clk_ctrl (hba , false, freq );
15151542}
15161543
15171544static int ufs_qcom_clk_scale_notify (struct ufs_hba * hba , bool scale_up ,
@@ -2092,11 +2119,53 @@ static int ufs_qcom_config_esi(struct ufs_hba *hba)
20922119 return 0 ;
20932120}
20942121
2122+ static unsigned long ufs_qcom_opp_freq_to_clk_freq (struct ufs_hba * hba ,
2123+ unsigned long freq , char * name )
2124+ {
2125+ struct ufs_clk_info * clki ;
2126+ struct dev_pm_opp * opp ;
2127+ unsigned long clk_freq ;
2128+ int idx = 0 ;
2129+ bool found = false;
2130+
2131+ opp = dev_pm_opp_find_freq_exact_indexed (hba -> dev , freq , 0 , true);
2132+ if (IS_ERR (opp )) {
2133+ dev_err (hba -> dev , "Failed to find OPP for exact frequency %lu\n" , freq );
2134+ return 0 ;
2135+ }
2136+
2137+ list_for_each_entry (clki , & hba -> clk_list_head , list ) {
2138+ if (!strcmp (clki -> name , name )) {
2139+ found = true;
2140+ break ;
2141+ }
2142+
2143+ idx ++ ;
2144+ }
2145+
2146+ if (!found ) {
2147+ dev_err (hba -> dev , "Failed to find clock '%s' in clk list\n" , name );
2148+ dev_pm_opp_put (opp );
2149+ return 0 ;
2150+ }
2151+
2152+ clk_freq = dev_pm_opp_get_freq_indexed (opp , idx );
2153+
2154+ dev_pm_opp_put (opp );
2155+
2156+ return clk_freq ;
2157+ }
2158+
20952159static u32 ufs_qcom_freq_to_gear_speed (struct ufs_hba * hba , unsigned long freq )
20962160{
2097- u32 gear = 0 ;
2161+ u32 gear = UFS_HS_DONT_CHANGE ;
2162+ unsigned long unipro_freq ;
20982163
2099- switch (freq ) {
2164+ if (!hba -> use_pm_opp )
2165+ return gear ;
2166+
2167+ unipro_freq = ufs_qcom_opp_freq_to_clk_freq (hba , freq , "core_clk_unipro" );
2168+ switch (unipro_freq ) {
21002169 case 403000000 :
21012170 gear = UFS_HS_G5 ;
21022171 break ;
@@ -2116,10 +2185,10 @@ static u32 ufs_qcom_freq_to_gear_speed(struct ufs_hba *hba, unsigned long freq)
21162185 break ;
21172186 default :
21182187 dev_err (hba -> dev , "%s: Unsupported clock freq : %lu\n" , __func__ , freq );
2119- break ;
2188+ return UFS_HS_DONT_CHANGE ;
21202189 }
21212190
2122- return gear ;
2191+ return min_t ( u32 , gear , hba -> max_pwr_info . info . gear_rx ) ;
21232192}
21242193
21252194/*
0 commit comments