@@ -122,7 +122,9 @@ static const struct {
122
122
};
123
123
124
124
static 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 );
126
128
127
129
static struct ufs_qcom_host * rcdev_to_ufs_host (struct reset_controller_dev * rcd )
128
130
{
@@ -506,10 +508,9 @@ static int ufs_qcom_power_up_sequence(struct ufs_hba *hba)
506
508
if (ret )
507
509
return ret ;
508
510
509
- if (phy -> power_count ) {
511
+ if (phy -> power_count )
510
512
phy_power_off (phy );
511
- phy_exit (phy );
512
- }
513
+
513
514
514
515
/* phy initialization - calibrate the phy */
515
516
ret = phy_init (phy );
@@ -597,13 +598,14 @@ static int ufs_qcom_hce_enable_notify(struct ufs_hba *hba,
597
598
*
598
599
* @hba: host controller instance
599
600
* @is_pre_scale_up: flag to check if pre scale up condition.
601
+ * @freq: target opp freq
600
602
* Return: zero for success and non-zero in case of a failure.
601
603
*/
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 )
603
605
{
604
606
struct ufs_qcom_host * host = ufshcd_get_variant (hba );
605
607
struct ufs_clk_info * clki ;
606
- unsigned long core_clk_rate = 0 ;
608
+ unsigned long clk_freq = 0 ;
607
609
u32 core_clk_cycles_per_us ;
608
610
609
611
/*
@@ -615,22 +617,34 @@ static int ufs_qcom_cfg_timers(struct ufs_hba *hba, bool is_pre_scale_up)
615
617
if (host -> hw_ver .major < 4 && !ufshcd_is_intr_aggr_allowed (hba ))
616
618
return 0 ;
617
619
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
+
618
626
list_for_each_entry (clki , & hba -> clk_list_head , list ) {
619
627
if (!strcmp (clki -> name , "core_clk" )) {
628
+ if (freq == ULONG_MAX ) {
629
+ clk_freq = clki -> max_freq ;
630
+ break ;
631
+ }
632
+
620
633
if (is_pre_scale_up )
621
- core_clk_rate = clki -> max_freq ;
634
+ clk_freq = clki -> max_freq ;
622
635
else
623
- core_clk_rate = clk_get_rate (clki -> clk );
636
+ clk_freq = clk_get_rate (clki -> clk );
624
637
break ;
625
638
}
626
639
627
640
}
628
641
642
+ cfg_timers :
629
643
/* 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 ;
632
646
633
- core_clk_cycles_per_us = core_clk_rate / USEC_PER_SEC ;
647
+ core_clk_cycles_per_us = clk_freq / USEC_PER_SEC ;
634
648
if (ufshcd_readl (hba , REG_UFS_SYS1CLK_1US ) != core_clk_cycles_per_us ) {
635
649
ufshcd_writel (hba , core_clk_cycles_per_us , REG_UFS_SYS1CLK_1US );
636
650
/*
@@ -650,13 +664,13 @@ static int ufs_qcom_link_startup_notify(struct ufs_hba *hba,
650
664
651
665
switch (status ) {
652
666
case PRE_CHANGE :
653
- if (ufs_qcom_cfg_timers (hba , false)) {
667
+ if (ufs_qcom_cfg_timers (hba , false, ULONG_MAX )) {
654
668
dev_err (hba -> dev , "%s: ufs_qcom_cfg_timers() failed\n" ,
655
669
__func__ );
656
670
return - EINVAL ;
657
671
}
658
672
659
- err = ufs_qcom_set_core_clk_ctrl (hba , ULONG_MAX );
673
+ err = ufs_qcom_set_core_clk_ctrl (hba , true, ULONG_MAX );
660
674
if (err )
661
675
dev_err (hba -> dev , "cfg core clk ctrl failed\n" );
662
676
/*
@@ -928,17 +942,6 @@ static int ufs_qcom_pwr_change_notify(struct ufs_hba *hba,
928
942
929
943
break ;
930
944
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
-
942
945
/* cache the power mode parameters to use internally */
943
946
memcpy (& host -> dev_req_params ,
944
947
dev_req_params , sizeof (* dev_req_params ));
@@ -1414,29 +1417,46 @@ static int ufs_qcom_set_clk_40ns_cycles(struct ufs_hba *hba,
1414
1417
return ufshcd_dme_set (hba , UIC_ARG_MIB (PA_VS_CORE_CLK_40NS_CYCLES ), reg );
1415
1418
}
1416
1419
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 )
1418
1421
{
1419
1422
struct ufs_qcom_host * host = ufshcd_get_variant (hba );
1420
1423
struct list_head * head = & hba -> clk_list_head ;
1421
1424
struct ufs_clk_info * clki ;
1422
1425
u32 cycles_in_1us = 0 ;
1423
1426
u32 core_clk_ctrl_reg ;
1427
+ unsigned long clk_freq ;
1424
1428
int err ;
1425
1429
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
+
1426
1438
list_for_each_entry (clki , head , list ) {
1427
1439
if (!IS_ERR_OR_NULL (clki -> clk ) &&
1428
1440
!strcmp (clki -> name , "core_clk_unipro" )) {
1429
- if (!clki -> max_freq )
1441
+ if (!clki -> max_freq ) {
1430
1442
cycles_in_1us = 150 ; /* default for backwards compatibility */
1431
- else if (freq == ULONG_MAX )
1443
+ break ;
1444
+ }
1445
+
1446
+ if (freq == ULONG_MAX ) {
1432
1447
cycles_in_1us = ceil (clki -> max_freq , HZ_PER_MHZ );
1433
- else
1434
- cycles_in_1us = ceil ( freq , HZ_PER_MHZ );
1448
+ break ;
1449
+ }
1435
1450
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 );
1436
1455
break ;
1437
1456
}
1438
1457
}
1439
1458
1459
+ set_core_clk_ctrl :
1440
1460
err = ufshcd_dme_get (hba ,
1441
1461
UIC_ARG_MIB (DME_VS_CORE_CLK_CTRL ),
1442
1462
& core_clk_ctrl_reg );
@@ -1473,13 +1493,13 @@ static int ufs_qcom_clk_scale_up_pre_change(struct ufs_hba *hba, unsigned long f
1473
1493
{
1474
1494
int ret ;
1475
1495
1476
- ret = ufs_qcom_cfg_timers (hba , true);
1496
+ ret = ufs_qcom_cfg_timers (hba , true, freq );
1477
1497
if (ret ) {
1478
1498
dev_err (hba -> dev , "%s ufs cfg timer failed\n" , __func__ );
1479
1499
return ret ;
1480
1500
}
1481
1501
/* 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 );
1483
1503
}
1484
1504
1485
1505
static 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)
1510
1530
1511
1531
static int ufs_qcom_clk_scale_down_post_change (struct ufs_hba * hba , unsigned long freq )
1512
1532
{
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
+ }
1513
1540
/* 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 );
1515
1542
}
1516
1543
1517
1544
static 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)
2092
2119
return 0 ;
2093
2120
}
2094
2121
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
+
2095
2159
static u32 ufs_qcom_freq_to_gear_speed (struct ufs_hba * hba , unsigned long freq )
2096
2160
{
2097
- u32 gear = 0 ;
2161
+ u32 gear = UFS_HS_DONT_CHANGE ;
2162
+ unsigned long unipro_freq ;
2098
2163
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 ) {
2100
2169
case 403000000 :
2101
2170
gear = UFS_HS_G5 ;
2102
2171
break ;
@@ -2116,10 +2185,10 @@ static u32 ufs_qcom_freq_to_gear_speed(struct ufs_hba *hba, unsigned long freq)
2116
2185
break ;
2117
2186
default :
2118
2187
dev_err (hba -> dev , "%s: Unsupported clock freq : %lu\n" , __func__ , freq );
2119
- break ;
2188
+ return UFS_HS_DONT_CHANGE ;
2120
2189
}
2121
2190
2122
- return gear ;
2191
+ return min_t ( u32 , gear , hba -> max_pwr_info . info . gear_rx ) ;
2123
2192
}
2124
2193
2125
2194
/*
0 commit comments