#include <linux/bitops.h>
#include <linux/delay.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <ufs/ufshcd.h>
#include <ufs/unipro.h>
#include "ufshcd-priv.h"
#define TX_EQ_SETTING_MASK 0x7
#define TX_EQ_SETTINGS_VALID_BIT BIT(15)
static bool use_adaptive_txeq;
module_param(use_adaptive_txeq, bool, 0644);
MODULE_PARM_DESC(use_adaptive_txeq, "Find and apply optimal TX Equalization settings before changing Power Mode (default: false)");
static int txeq_gear_set(const char *val, const struct kernel_param *kp)
{
return param_set_uint_minmax(val, kp, UFS_HS_G1, UFS_HS_GEAR_MAX);
}
static const struct kernel_param_ops txeq_gear_ops = {
.set = txeq_gear_set,
.get = param_get_uint,
};
static unsigned int adaptive_txeq_gear = UFS_HS_G6;
module_param_cb(adaptive_txeq_gear, &txeq_gear_ops, &adaptive_txeq_gear, 0644);
MODULE_PARM_DESC(adaptive_txeq_gear, "For HS-Gear[n] and above, adaptive txeq shall be used");
static bool use_txeq_presets;
module_param(use_txeq_presets, bool, 0644);
MODULE_PARM_DESC(use_txeq_presets, "Use only the 8 TX Equalization Presets (pre-defined Pre-Shoot & De-Emphasis combinations) for TX EQTR (default: false)");
static bool txeq_presets_selected[UFS_TX_EQ_PRESET_MAX] = {[0 ... (UFS_TX_EQ_PRESET_MAX - 1)] = 1};
module_param_array(txeq_presets_selected, bool, NULL, 0644);
MODULE_PARM_DESC(txeq_presets_selected, "Use only the selected Presets out of the 8 TX Equalization Presets for TX EQTR");
static int txeq_setting_sel_set(const char *val, const struct kernel_param *kp)
{
return param_set_uint_minmax(val, kp, 0, 1);
}
static const struct kernel_param_ops txeq_setting_sel_ops = {
.set = txeq_setting_sel_set,
.get = param_get_uint,
};
static unsigned int txeq_setting_sel;
module_param_cb(txeq_setting_sel, &txeq_setting_sel_ops, &txeq_setting_sel, 0644);
MODULE_PARM_DESC(txeq_setting_sel, "The qTxEQGnSettings and wTxEQGnSettingsExt Attributes selector used to retrieve and store TX Equalization settings");
static bool retrieve_txeq_setting = true;
module_param(retrieve_txeq_setting, bool, 0644);
MODULE_PARM_DESC(retrieve_txeq_setting, "Retrieve TX Equalization settings from qTxEQGnSettings and wTxEQGnSettingsExt Attributes (default: true)");
static bool store_txeq_setting = true;
module_param(store_txeq_setting, bool, 0644);
MODULE_PARM_DESC(store_txeq_setting, "Store the optimal TX Equalization settings to qTxEQGnSettings and wTxEQGnSettingsExt Attributes (default: true)");
static const struct __ufs_tx_eq_preset {
u8 preshoot;
u8 deemphasis;
} ufs_tx_eq_preset[UFS_TX_EQ_PRESET_MAX] = {
[UFS_TX_EQ_PRESET_P0] = {UFS_TX_HS_PRESHOOT_DB_0P0, UFS_TX_HS_DEEMPHASIS_DB_0P0},
[UFS_TX_EQ_PRESET_P1] = {UFS_TX_HS_PRESHOOT_DB_0P0, UFS_TX_HS_DEEMPHASIS_DB_0P8},
[UFS_TX_EQ_PRESET_P2] = {UFS_TX_HS_PRESHOOT_DB_0P0, UFS_TX_HS_DEEMPHASIS_DB_1P6},
[UFS_TX_EQ_PRESET_P3] = {UFS_TX_HS_PRESHOOT_DB_0P8, UFS_TX_HS_DEEMPHASIS_DB_0P0},
[UFS_TX_EQ_PRESET_P4] = {UFS_TX_HS_PRESHOOT_DB_1P6, UFS_TX_HS_DEEMPHASIS_DB_0P0},
[UFS_TX_EQ_PRESET_P5] = {UFS_TX_HS_PRESHOOT_DB_0P8, UFS_TX_HS_DEEMPHASIS_DB_0P8},
[UFS_TX_EQ_PRESET_P6] = {UFS_TX_HS_PRESHOOT_DB_0P8, UFS_TX_HS_DEEMPHASIS_DB_1P6},
[UFS_TX_EQ_PRESET_P7] = {UFS_TX_HS_PRESHOOT_DB_1P6, UFS_TX_HS_DEEMPHASIS_DB_0P8},
};
static const u32 pa_peer_rx_adapt_initial[UFS_HS_GEAR_MAX] = {
0,
0,
0,
PA_PEERRXHSG4ADAPTINITIAL,
PA_PEERRXHSG5ADAPTINITIAL,
PA_PEERRXHSG6ADAPTINITIALL0L3
};
static const u32 rx_adapt_initial_cap[UFS_HS_GEAR_MAX] = {
0,
0,
0,
RX_HS_G4_ADAPT_INITIAL_CAP,
RX_HS_G5_ADAPT_INITIAL_CAP,
RX_HS_G6_ADAPT_INITIAL_CAP
};
static const u32 pa_tx_eq_setting[UFS_HS_GEAR_MAX] = {
PA_TXEQG1SETTING,
PA_TXEQG2SETTING,
PA_TXEQG3SETTING,
PA_TXEQG4SETTING,
PA_TXEQG5SETTING,
PA_TXEQG6SETTING
};
static inline u8 tx_eq_device_preshoot_decode(u64 eq, u8 lane)
{
return (u8)((eq >> (lane * TX_HS_PRESHOOT_SHIFT)) & TX_EQ_SETTING_MASK);
}
static inline u8 tx_eq_device_deemphasis_decode(u64 eq, u8 lane)
{
return (u8)((eq >> (lane * TX_HS_DEEMPHASIS_SHIFT + 16)) & TX_EQ_SETTING_MASK);
}
static inline u8 tx_eq_host_preshoot_decode(u64 eq, u8 lane)
{
return (u8)((eq >> (lane * TX_HS_PRESHOOT_SHIFT + 32)) & TX_EQ_SETTING_MASK);
}
static inline u8 tx_eq_host_deemphasis_decode(u64 eq, u8 lane)
{
return (u8)((eq >> (lane * TX_HS_DEEMPHASIS_SHIFT + 48)) & TX_EQ_SETTING_MASK);
}
static inline bool tx_eq_device_precode_en_decode(u16 eq_ext, u8 lane)
{
return eq_ext & BIT(lane);
}
static inline bool tx_eq_host_precode_en_decode(u16 eq_ext, u8 lane)
{
return eq_ext & BIT(lane + 4);
}
static inline u64 tx_eq_device_preshoot_encode(u64 val, u8 lane)
{
return (val & TX_EQ_SETTING_MASK) << (lane * TX_HS_PRESHOOT_SHIFT);
}
static inline u64 tx_eq_device_deemphasis_encode(u64 val, u8 lane)
{
return (val & TX_EQ_SETTING_MASK) << (lane * TX_HS_DEEMPHASIS_SHIFT + 16);
}
static inline u64 tx_eq_host_preshoot_encode(u64 val, u8 lane)
{
return (val & TX_EQ_SETTING_MASK) << (lane * TX_HS_PRESHOOT_SHIFT + 32);
}
static inline u64 tx_eq_host_deemphasis_encode(u64 val, u8 lane)
{
return (val & TX_EQ_SETTING_MASK) << (lane * TX_HS_DEEMPHASIS_SHIFT + 48);
}
static inline u16 tx_eq_device_precode_en_encode(bool en, u8 lane)
{
return (u16)en << lane;
}
static inline u16 tx_eq_host_precode_en_encode(bool en, u8 lane)
{
return (u16)en << (lane + 4);
}
static int ufshcd_configure_precoding(struct ufs_hba *hba,
struct ufshcd_tx_eq_params *params)
{
struct ufs_pa_layer_attr *pwr_info = &hba->max_pwr_info.info;
u32 local_precode_en = 0;
u32 peer_precode_en = 0;
int lane, ret;
for (lane = 0; lane < pwr_info->lane_tx; lane++) {
if (params->host[lane].precode_en) {
local_precode_en |= PRECODEEN_TX_BIT(lane);
peer_precode_en |= PRECODEEN_RX_BIT(lane);
}
}
for (lane = 0; lane < pwr_info->lane_rx; lane++) {
if (params->device[lane].precode_en) {
peer_precode_en |= PRECODEEN_TX_BIT(lane);
local_precode_en |= PRECODEEN_RX_BIT(lane);
}
}
if (!local_precode_en && !peer_precode_en) {
dev_dbg(hba->dev, "Pre-Coding is not required for Host and Device\n");
return 0;
}
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(PA_PRECODEEN), local_precode_en);
if (ret) {
dev_err(hba->dev, "Failed to set local PA_PreCodeEn: %d\n", ret);
return ret;
}
ret = ufshcd_dme_peer_set(hba, UIC_ARG_MIB(PA_PRECODEEN), peer_precode_en);
if (ret) {
dev_err(hba->dev, "Failed to set peer PA_PreCodeEn: %d\n", ret);
return ret;
}
dev_dbg(hba->dev, "Local PA_PreCodeEn: 0x%02x, Peer PA_PreCodeEn: 0x%02x\n",
local_precode_en, peer_precode_en);
return 0;
}
void ufshcd_print_tx_eq_params(struct ufs_hba *hba)
{
struct ufs_pa_layer_attr *pwr_info = &hba->max_pwr_info.info;
struct ufshcd_tx_eq_params *params;
u32 gear = hba->pwr_info.gear_tx;
int lane;
if (!ufshcd_is_tx_eq_supported(hba))
return;
if (gear < UFS_HS_G1 || gear > UFS_HS_GEAR_MAX)
return;
params = &hba->tx_eq_params[gear - 1];
if (!params->is_valid || !params->is_applied)
return;
for (lane = 0; lane < pwr_info->lane_tx; lane++)
dev_dbg(hba->dev, "Host TX Lane %d: PreShoot %u, DeEmphasis %u, FOM %u, PreCodeEn %d\n",
lane, params->host[lane].preshoot,
params->host[lane].deemphasis,
params->host[lane].fom_val,
params->host[lane].precode_en);
for (lane = 0; lane < pwr_info->lane_rx; lane++)
dev_dbg(hba->dev, "Device TX Lane %d: PreShoot %u, DeEmphasis %u, FOM %u, PreCodeEn %d\n",
lane, params->device[lane].preshoot,
params->device[lane].deemphasis,
params->device[lane].fom_val,
params->device[lane].precode_en);
}
static inline u32
ufshcd_compose_tx_eq_setting(struct ufshcd_tx_eq_settings *settings,
int num_lanes)
{
u32 setting = 0;
int lane;
for (lane = 0; lane < num_lanes; lane++, settings++) {
setting |= TX_HS_PRESHOOT_BITS(lane, settings->preshoot);
setting |= TX_HS_DEEMPHASIS_BITS(lane, settings->deemphasis);
}
return setting;
}
int ufshcd_apply_tx_eq_settings(struct ufs_hba *hba,
struct ufshcd_tx_eq_params *params, u32 gear)
{
struct ufs_pa_layer_attr *pwr_info = &hba->max_pwr_info.info;
u32 setting;
int ret;
setting = ufshcd_compose_tx_eq_setting(params->host, pwr_info->lane_tx);
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(pa_tx_eq_setting[gear - 1]), setting);
if (ret)
return ret;
setting = ufshcd_compose_tx_eq_setting(params->device, pwr_info->lane_rx);
ret = ufshcd_dme_peer_set(hba, UIC_ARG_MIB(pa_tx_eq_setting[gear - 1]), setting);
if (ret)
return ret;
if (gear >= UFS_HS_G6) {
ret = ufshcd_configure_precoding(hba, params);
if (ret) {
dev_err(hba->dev, "Failed to configure pre-coding: %d\n", ret);
return ret;
}
}
return 0;
}
EXPORT_SYMBOL_GPL(ufshcd_apply_tx_eq_settings);
static void ufshcd_evaluate_tx_eqtr_fom(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode,
struct ufshcd_tx_eqtr_data *eqtr_data,
struct tx_eqtr_iter *h_iter,
struct tx_eqtr_iter *d_iter)
{
u8 preshoot, deemphasis, fom_value;
bool precode_en;
int lane;
for (lane = 0; h_iter->is_updated && lane < pwr_mode->lane_tx; lane++) {
preshoot = h_iter->preshoot;
deemphasis = h_iter->deemphasis;
fom_value = h_iter->fom[lane] & RX_FOM_VALUE_MASK;
precode_en = h_iter->fom[lane] & RX_FOM_PRECODING_EN_BIT;
eqtr_data->host_fom[lane][preshoot][deemphasis] = h_iter->fom[lane];
if (fom_value > eqtr_data->host[lane].fom_val) {
eqtr_data->host[lane].preshoot = preshoot;
eqtr_data->host[lane].deemphasis = deemphasis;
eqtr_data->host[lane].fom_val = fom_value;
eqtr_data->host[lane].precode_en = precode_en;
}
dev_dbg(hba->dev, "TX EQTR: Host TX Lane %d: PreShoot %u, DeEmphasis %u, FOM value %u, PreCodeEn %d\n",
lane, preshoot, deemphasis, fom_value, precode_en);
}
for (lane = 0; d_iter->is_updated && lane < pwr_mode->lane_rx; lane++) {
preshoot = d_iter->preshoot;
deemphasis = d_iter->deemphasis;
fom_value = d_iter->fom[lane] & RX_FOM_VALUE_MASK;
precode_en = d_iter->fom[lane] & RX_FOM_PRECODING_EN_BIT;
eqtr_data->device_fom[lane][preshoot][deemphasis] = d_iter->fom[lane];
if (fom_value > eqtr_data->device[lane].fom_val) {
eqtr_data->device[lane].preshoot = preshoot;
eqtr_data->device[lane].deemphasis = deemphasis;
eqtr_data->device[lane].fom_val = fom_value;
eqtr_data->device[lane].precode_en = precode_en;
}
dev_dbg(hba->dev, "TX EQTR: Device TX Lane %d: PreShoot %u, DeEmphasis %u, FOM value %u, PreCodeEn %d\n",
lane, preshoot, deemphasis, fom_value, precode_en);
}
}
static int ufshcd_get_rx_fom(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode,
struct tx_eqtr_iter *h_iter,
struct tx_eqtr_iter *d_iter)
{
int lane, ret;
u32 fom;
for (lane = 0; lane < pwr_mode->lane_tx; lane++) {
ret = ufshcd_dme_peer_get(hba, UIC_ARG_MIB_SEL(RX_FOM,
UIC_ARG_MPHY_RX_GEN_SEL_INDEX(lane)),
&fom);
if (ret)
return ret;
h_iter->fom[lane] = (u8)fom;
}
for (lane = 0; lane < pwr_mode->lane_rx; lane++) {
ret = ufshcd_dme_get(hba, UIC_ARG_MIB_SEL(RX_FOM,
UIC_ARG_MPHY_RX_GEN_SEL_INDEX(lane)),
&fom);
if (ret)
return ret;
d_iter->fom[lane] = (u8)fom;
}
ret = ufshcd_vops_get_rx_fom(hba, pwr_mode, h_iter, d_iter);
if (ret)
dev_err(hba->dev, "Failed to get FOM via vops: %d\n", ret);
return ret;
}
bool ufshcd_is_txeq_presets_used(struct ufs_hba *hba)
{
return use_txeq_presets;
}
bool ufshcd_is_txeq_preset_selected(u8 preshoot, u8 deemphasis)
{
int i;
for (i = 0; i < UFS_TX_EQ_PRESET_MAX; i++) {
if (!txeq_presets_selected[i])
continue;
if (preshoot == ufs_tx_eq_preset[i].preshoot &&
deemphasis == ufs_tx_eq_preset[i].deemphasis)
return true;
}
return false;
}
static void tx_eqtr_iter_try_update(struct tx_eqtr_iter *iter,
u8 preshoot, u8 deemphasis)
{
if (!test_bit(preshoot, &iter->preshoot_bitmap) ||
!test_bit(deemphasis, &iter->deemphasis_bitmap) ||
(use_txeq_presets && !ufshcd_is_txeq_preset_selected(preshoot, deemphasis))) {
iter->is_updated = false;
return;
}
iter->preshoot = preshoot;
iter->deemphasis = deemphasis;
iter->is_updated = true;
}
static bool tx_eqtr_iter_update(u8 preshoot, u8 deemphasis,
struct tx_eqtr_iter *h_iter,
struct tx_eqtr_iter *d_iter)
{
tx_eqtr_iter_try_update(h_iter, preshoot, deemphasis);
tx_eqtr_iter_try_update(d_iter, preshoot, deemphasis);
return h_iter->is_updated || d_iter->is_updated;
}
static int ufshcd_tx_eqtr_iter_init(struct ufs_hba *hba,
struct tx_eqtr_iter *h_iter,
struct tx_eqtr_iter *d_iter)
{
u32 cap;
int ret;
if (!hba->host_preshoot_cap) {
ret = ufshcd_dme_get(hba, UIC_ARG_MIB(TX_HS_PRESHOOT_SETTING_CAP), &cap);
if (ret)
return ret;
hba->host_preshoot_cap = cap & TX_EQTR_CAP_MASK;
}
if (!hba->host_deemphasis_cap) {
ret = ufshcd_dme_get(hba, UIC_ARG_MIB(TX_HS_DEEMPHASIS_SETTING_CAP), &cap);
if (ret)
return ret;
hba->host_deemphasis_cap = cap & TX_EQTR_CAP_MASK;
}
if (!hba->device_preshoot_cap) {
ret = ufshcd_dme_peer_get(hba, UIC_ARG_MIB(TX_HS_PRESHOOT_SETTING_CAP), &cap);
if (ret)
return ret;
hba->device_preshoot_cap = cap & TX_EQTR_CAP_MASK;
}
if (!hba->device_deemphasis_cap) {
ret = ufshcd_dme_peer_get(hba, UIC_ARG_MIB(TX_HS_DEEMPHASIS_SETTING_CAP), &cap);
if (ret)
return ret;
hba->device_deemphasis_cap = cap & TX_EQTR_CAP_MASK;
}
h_iter->preshoot_bitmap = (hba->host_preshoot_cap << 0x1) | 0x1;
h_iter->deemphasis_bitmap = (hba->host_deemphasis_cap << 0x1) | 0x1;
d_iter->preshoot_bitmap = (hba->device_preshoot_cap << 0x1) | 0x1;
d_iter->deemphasis_bitmap = (hba->device_deemphasis_cap << 0x1) | 0x1;
return 0;
}
static inline u64 adapt_cap_to_t_adapt(u32 adapt_cap)
{
u64 tadapt;
u8 adapt_length = adapt_cap & ADAPT_LENGTH_MASK;
if (!IS_ADAPT_RANGE_COARSE(adapt_cap))
tadapt = TADAPT_FACTOR * (adapt_length + 1);
else
tadapt = TADAPT_FACTOR * (1 << adapt_length);
return tadapt;
}
static inline u64 adapt_cap_to_t_adapt_l0l3(u32 adapt_cap)
{
u64 tadapt;
u8 adapt_length = adapt_cap & ADAPT_LENGTH_MASK;
if (!IS_ADAPT_RANGE_COARSE(adapt_cap))
tadapt = TADAPT_L0L3_FACTOR * adapt_length;
else
tadapt = TADAPT_L0L3_FACTOR * (1 << adapt_length);
return tadapt;
}
static inline u64 adapt_cap_to_t_adapt_l0l1l2l3(u32 adapt_cap)
{
u64 tadapt;
u8 adapt_length = adapt_cap & ADAPT_LENGTH_MASK;
if (!IS_ADAPT_RANGE_COARSE(adapt_cap))
tadapt = TADAPT_L0L1L2L3_FACTOR * (adapt_length + 1);
else
tadapt = TADAPT_L0L1L2L3_FACTOR * (1 << adapt_length);
return tadapt;
}
static int ufshcd_setup_tx_eqtr_adapt_length(struct ufs_hba *hba,
struct ufshcd_tx_eq_params *params,
u32 gear)
{
struct ufshcd_tx_eqtr_record *rec = params->eqtr_record;
u32 adapt_eqtr;
int ret;
if (rec && rec->saved_adapt_eqtr) {
adapt_eqtr = rec->saved_adapt_eqtr;
goto set_adapt_eqtr;
}
if (gear == UFS_HS_G4 || gear == UFS_HS_G5) {
u64 t_adapt, t_adapt_local, t_adapt_peer;
u32 adapt_cap_local, adapt_cap_peer, adapt_length;
ret = ufshcd_dme_get(hba, UIC_ARG_MIB_SEL(rx_adapt_initial_cap[gear - 1],
UIC_ARG_MPHY_RX_GEN_SEL_INDEX(0)),
&adapt_cap_local);
if (ret)
return ret;
if (adapt_cap_local > ADAPT_LENGTH_MAX) {
dev_err(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_CAP (0x%x) exceeds MAX\n",
gear, adapt_cap_local);
return -EINVAL;
}
ret = ufshcd_dme_get(hba, UIC_ARG_MIB(pa_peer_rx_adapt_initial[gear - 1]),
&adapt_cap_peer);
if (ret)
return ret;
if (adapt_cap_peer > ADAPT_LENGTH_MAX) {
dev_err(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_CAP (0x%x) exceeds MAX\n",
gear, adapt_cap_peer);
return -EINVAL;
}
t_adapt_local = adapt_cap_to_t_adapt(adapt_cap_local);
t_adapt_peer = adapt_cap_to_t_adapt(adapt_cap_peer);
t_adapt = max(t_adapt_local, t_adapt_peer);
dev_dbg(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_CAP = 0x%x\n",
gear, adapt_cap_local);
dev_dbg(hba->dev, "peer RX_HS_G%u_ADAPT_INITIAL_CAP = 0x%x\n",
gear, adapt_cap_peer);
dev_dbg(hba->dev, "t_adapt_local = %llu UI, t_adapt_peer = %llu UI\n",
t_adapt_local, t_adapt_peer);
dev_dbg(hba->dev, "TAdapt %llu UI selected for TX EQTR\n",
t_adapt);
adapt_length = (t_adapt_local >= t_adapt_peer) ?
adapt_cap_local : adapt_cap_peer;
if (gear == UFS_HS_G4 && t_adapt < TX_EQTR_HS_G4_MIN_T_ADAPT) {
dev_dbg(hba->dev, "TAdapt %llu UI is too short for TX EQTR for HS-G%u, use default Adapt 0x%x\n",
t_adapt, gear, TX_EQTR_HS_G4_ADAPT_DEFAULT);
adapt_length = TX_EQTR_HS_G4_ADAPT_DEFAULT;
} else if (gear == UFS_HS_G5 && t_adapt < TX_EQTR_HS_G5_MIN_T_ADAPT) {
dev_dbg(hba->dev, "TAdapt %llu UI is too short for TX EQTR for HS-G%u, use default Adapt 0x%x\n",
t_adapt, gear, TX_EQTR_HS_G5_ADAPT_DEFAULT);
adapt_length = TX_EQTR_HS_G5_ADAPT_DEFAULT;
}
adapt_eqtr = adapt_length |
(TX_EQTR_ADAPT_RESERVED << TX_EQTR_ADAPT_LENGTH_L0L1L2L3_SHIFT);
} else if (gear == UFS_HS_G6) {
u64 t_adapt, t_adapt_l0l3, t_adapt_l0l3_local, t_adapt_l0l3_peer;
u64 t_adapt_l0l1l2l3, t_adapt_l0l1l2l3_local, t_adapt_l0l1l2l3_peer;
u32 adapt_l0l3_cap_local, adapt_l0l3_cap_peer, adapt_length_l0l3;
u32 adapt_l0l1l2l3_cap_local, adapt_l0l1l2l3_cap_peer, adapt_length_l0l1l2l3;
ret = ufshcd_dme_get(hba, UIC_ARG_MIB_SEL(rx_adapt_initial_cap[gear - 1],
UIC_ARG_MPHY_RX_GEN_SEL_INDEX(0)),
&adapt_l0l3_cap_local);
if (ret)
return ret;
if (adapt_l0l3_cap_local > ADAPT_L0L3_LENGTH_MAX) {
dev_err(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_CAP (0x%x) exceeds MAX\n",
gear, adapt_l0l3_cap_local);
return -EINVAL;
}
ret = ufshcd_dme_get(hba, UIC_ARG_MIB(pa_peer_rx_adapt_initial[gear - 1]),
&adapt_l0l3_cap_peer);
if (ret)
return ret;
if (adapt_l0l3_cap_peer > ADAPT_L0L3_LENGTH_MAX) {
dev_err(hba->dev, "peer RX_HS_G%u_ADAPT_INITIAL_CAP (0x%x) exceeds MAX\n",
gear, adapt_l0l3_cap_peer);
return -EINVAL;
}
t_adapt_l0l3_local = adapt_cap_to_t_adapt_l0l3(adapt_l0l3_cap_local);
t_adapt_l0l3_peer = adapt_cap_to_t_adapt_l0l3(adapt_l0l3_cap_peer);
dev_dbg(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_CAP = 0x%x\n",
gear, adapt_l0l3_cap_local);
dev_dbg(hba->dev, "peer RX_HS_G%u_ADAPT_INITIAL_CAP = 0x%x\n",
gear, adapt_l0l3_cap_peer);
dev_dbg(hba->dev, "t_adapt_l0l3_local = %llu UI, t_adapt_l0l3_peer = %llu UI\n",
t_adapt_l0l3_local, t_adapt_l0l3_peer);
ret = ufshcd_dme_get(hba, UIC_ARG_MIB_SEL(RX_HS_G6_ADAPT_INITIAL_L0L1L2L3_CAP,
UIC_ARG_MPHY_RX_GEN_SEL_INDEX(0)),
&adapt_l0l1l2l3_cap_local);
if (ret)
return ret;
if (adapt_l0l1l2l3_cap_local > ADAPT_L0L1L2L3_LENGTH_MAX) {
dev_err(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_L0L1L2L3_CAP (0x%x) exceeds MAX\n",
gear, adapt_l0l1l2l3_cap_local);
if (!(hba->quirks & UFSHCD_QUIRK_EXTENDED_TX_EQTR_ADAPT_LENGTH_L0L1L2L3))
return -EINVAL;
}
ret = ufshcd_dme_get(hba, UIC_ARG_MIB(PA_PEERRXHSG6ADAPTINITIALL0L1L2L3),
&adapt_l0l1l2l3_cap_peer);
if (ret)
return ret;
if (adapt_l0l1l2l3_cap_peer > ADAPT_L0L1L2L3_LENGTH_MAX) {
dev_err(hba->dev, "peer RX_HS_G%u_ADAPT_INITIAL_L0L1L2L3_CAP (0x%x) exceeds MAX\n",
gear, adapt_l0l1l2l3_cap_peer);
if (!(hba->quirks & UFSHCD_QUIRK_EXTENDED_TX_EQTR_ADAPT_LENGTH_L0L1L2L3))
return -EINVAL;
}
t_adapt_l0l1l2l3_local = adapt_cap_to_t_adapt_l0l1l2l3(adapt_l0l1l2l3_cap_local);
t_adapt_l0l1l2l3_peer = adapt_cap_to_t_adapt_l0l1l2l3(adapt_l0l1l2l3_cap_peer);
dev_dbg(hba->dev, "local RX_HS_G%u_ADAPT_INITIAL_L0L1L2L3_CAP = 0x%x\n",
gear, adapt_l0l1l2l3_cap_local);
dev_dbg(hba->dev, "peer RX_HS_G%u_ADAPT_INITIAL_L0L1L2L3_CAP = 0x%x\n",
gear, adapt_l0l1l2l3_cap_peer);
dev_dbg(hba->dev, "t_adapt_l0l1l2l3_local = %llu UI, t_adapt_l0l1l2l3_peer = %llu UI\n",
t_adapt_l0l1l2l3_local, t_adapt_l0l1l2l3_peer);
t_adapt_l0l1l2l3 = max(t_adapt_l0l1l2l3_local, t_adapt_l0l1l2l3_peer);
t_adapt_l0l3 = max(t_adapt_l0l3_local, t_adapt_l0l3_peer);
t_adapt = t_adapt_l0l3 + t_adapt_l0l1l2l3;
dev_dbg(hba->dev, "TAdapt %llu PAM-4 UI selected for TX EQTR\n",
t_adapt);
adapt_length_l0l3 = (t_adapt_l0l3_local >= t_adapt_l0l3_peer) ?
adapt_l0l3_cap_local : adapt_l0l3_cap_peer;
adapt_length_l0l1l2l3 = (t_adapt_l0l1l2l3_local >= t_adapt_l0l1l2l3_peer) ?
adapt_l0l1l2l3_cap_local : adapt_l0l1l2l3_cap_peer;
if (t_adapt < TX_EQTR_HS_G6_MIN_T_ADAPT) {
dev_dbg(hba->dev, "TAdapt %llu UI is too short for TX EQTR for HS-G%u, use default Adapt 0x%x\n",
t_adapt, gear, TX_EQTR_HS_G6_ADAPT_DEFAULT);
adapt_length_l0l3 = TX_EQTR_HS_G6_ADAPT_DEFAULT;
}
adapt_eqtr = adapt_length_l0l3 |
(adapt_length_l0l1l2l3 << TX_EQTR_ADAPT_LENGTH_L0L1L2L3_SHIFT);
} else {
return -EINVAL;
}
if (rec)
rec->saved_adapt_eqtr = (u16)adapt_eqtr;
set_adapt_eqtr:
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXADAPTLENGTH_EQTR), adapt_eqtr);
if (ret)
dev_err(hba->dev, "Failed to set adapt length for TX EQTR: %d\n", ret);
else
dev_dbg(hba->dev, "PA_TXADAPTLENGTH_EQTR configured to 0x%08x\n", adapt_eqtr);
return ret;
}
static inline u32 ufshcd_compose_tx_eqtr_setting(struct tx_eqtr_iter *iter,
int num_lanes)
{
u32 setting = 0;
int lane;
for (lane = 0; lane < num_lanes; lane++) {
setting |= TX_HS_PRESHOOT_BITS(lane, iter->preshoot);
setting |= TX_HS_DEEMPHASIS_BITS(lane, iter->deemphasis);
}
return setting;
}
static int ufshcd_apply_tx_eqtr_settings(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode,
struct tx_eqtr_iter *h_iter,
struct tx_eqtr_iter *d_iter)
{
u32 setting;
int ret;
setting = ufshcd_compose_tx_eqtr_setting(h_iter, pwr_mode->lane_tx);
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXEQTRSETTING), setting);
if (ret)
return ret;
setting = ufshcd_compose_tx_eqtr_setting(d_iter, pwr_mode->lane_rx);
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(PA_PEERTXEQTRSETTING), setting);
if (ret)
return ret;
ret = ufshcd_vops_apply_tx_eqtr_settings(hba, pwr_mode, h_iter, d_iter);
return ret;
}
static inline void
ufshcd_update_tx_eq_params(struct ufshcd_tx_eq_params *params,
struct ufs_pa_layer_attr *pwr_mode,
struct ufshcd_tx_eqtr_data *eqtr_data)
{
struct ufshcd_tx_eqtr_record *rec = params->eqtr_record;
if (params->is_valid) {
int lane;
for (lane = 0; lane < pwr_mode->lane_tx; lane++)
if (eqtr_data->host[lane].fom_val == 0)
eqtr_data->host[lane] = params->host[lane];
for (lane = 0; lane < pwr_mode->lane_rx; lane++)
if (eqtr_data->device[lane].fom_val == 0)
eqtr_data->device[lane] = params->device[lane];
}
memcpy(params->host, eqtr_data->host, sizeof(params->host));
memcpy(params->device, eqtr_data->device, sizeof(params->device));
if (!rec)
return;
memcpy(rec->host_fom, eqtr_data->host_fom, sizeof(rec->host_fom));
memcpy(rec->device_fom, eqtr_data->device_fom, sizeof(rec->device_fom));
rec->last_record_ts = ktime_get();
rec->last_record_index++;
}
static int __ufshcd_tx_eqtr(struct ufs_hba *hba,
struct ufshcd_tx_eq_params *params,
struct ufs_pa_layer_attr *pwr_mode)
{
struct ufshcd_tx_eqtr_data *eqtr_data __free(kfree) =
kzalloc(sizeof(*eqtr_data), GFP_KERNEL);
struct tx_eqtr_iter h_iter = {};
struct tx_eqtr_iter d_iter = {};
u32 gear = pwr_mode->gear_tx;
u8 preshoot, deemphasis;
ktime_t start;
int ret;
if (!eqtr_data)
return -ENOMEM;
dev_info(hba->dev, "Start TX EQTR procedure for HS-G%u, Rate-%s, RX Lanes: %u, TX Lanes: %u\n",
gear, ufs_hs_rate_to_str(pwr_mode->hs_rate),
pwr_mode->lane_rx, pwr_mode->lane_tx);
start = ktime_get();
ret = ufshcd_setup_tx_eqtr_adapt_length(hba, params, gear);
if (ret) {
dev_err(hba->dev, "Failed to setup TX EQTR Adaptation length: %d\n", ret);
return ret;
}
ret = ufshcd_tx_eqtr_iter_init(hba, &h_iter, &d_iter);
if (ret) {
dev_err(hba->dev, "Failed to init TX EQTR data: %d\n", ret);
return ret;
}
for (preshoot = 0; preshoot < TX_HS_NUM_PRESHOOT; preshoot++) {
for (deemphasis = 0; deemphasis < TX_HS_NUM_DEEMPHASIS; deemphasis++) {
if (!tx_eqtr_iter_update(preshoot, deemphasis, &h_iter, &d_iter))
continue;
ret = ufshcd_apply_tx_eqtr_settings(hba, pwr_mode, &h_iter, &d_iter);
if (ret) {
dev_err(hba->dev, "Failed to apply TX EQTR settings (PreShoot %u, DeEmphasis %u): %d\n",
preshoot, deemphasis, ret);
return ret;
}
ret = ufshcd_uic_tx_eqtr(hba, gear);
if (ret) {
dev_err(hba->dev, "Failed to trigger UIC TX EQTR for target gear %u: %d\n",
gear, ret);
return ret;
}
ret = ufshcd_get_rx_fom(hba, pwr_mode, &h_iter, &d_iter);
if (ret) {
dev_err(hba->dev, "Failed to get RX_FOM: %d\n",
ret);
return ret;
}
ufshcd_evaluate_tx_eqtr_fom(hba, pwr_mode, eqtr_data, &h_iter, &d_iter);
}
}
dev_info(hba->dev, "TX EQTR procedure completed! Time elapsed: %llu ms\n",
ktime_to_ms(ktime_sub(ktime_get(), start)));
ufshcd_update_tx_eq_params(params, pwr_mode, eqtr_data);
return ret;
}
static int ufshcd_tx_eqtr_prepare(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode)
{
struct ufs_pa_layer_attr pwr_mode_hs_g1 = {
.gear_rx = UFS_HS_G1,
.gear_tx = UFS_HS_G1,
.lane_rx = pwr_mode->lane_rx,
.lane_tx = pwr_mode->lane_tx,
.pwr_rx = FAST_MODE,
.pwr_tx = FAST_MODE,
.hs_rate = pwr_mode->hs_rate,
};
u32 rate = pwr_mode->hs_rate;
int ret;
ret = ufshcd_change_power_mode(hba, &pwr_mode_hs_g1,
UFSHCD_PMC_POLICY_DONT_FORCE);
if (ret) {
dev_err(hba->dev, "TX EQTR: Failed to change power mode to HS-G1, Rate-%s: %d\n",
ufs_hs_rate_to_str(rate), ret);
return ret;
}
ret = ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXHSADAPTTYPE),
PA_INITIAL_ADAPT);
if (ret)
dev_err(hba->dev, "TX EQTR: Failed to set Host Adapt type to INITIAL: %d\n",
ret);
return ret;
}
static void ufshcd_tx_eqtr_unprepare(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode)
{
int err;
if (pwr_mode->pwr_rx == SLOWAUTO_MODE || pwr_mode->hs_rate == 0)
return;
err = ufshcd_change_power_mode(hba, pwr_mode,
UFSHCD_PMC_POLICY_DONT_FORCE);
if (err)
dev_err(hba->dev, "%s: Failed to restore Power Mode: %d\n",
__func__, err);
}
static int ufshcd_tx_eqtr(struct ufs_hba *hba,
struct ufshcd_tx_eq_params *params,
struct ufs_pa_layer_attr *pwr_mode)
{
struct ufs_pa_layer_attr old_pwr_info;
int ret;
if (!params->eqtr_record) {
params->eqtr_record = devm_kzalloc(hba->dev,
sizeof(*params->eqtr_record),
GFP_KERNEL);
if (!params->eqtr_record)
return -ENOMEM;
}
memcpy(&old_pwr_info, &hba->pwr_info, sizeof(struct ufs_pa_layer_attr));
ret = ufshcd_tx_eqtr_prepare(hba, pwr_mode);
if (ret) {
dev_err(hba->dev, "Failed to prepare TX EQTR: %d\n", ret);
goto out;
}
ret = ufshcd_vops_tx_eqtr_notify(hba, PRE_CHANGE, pwr_mode);
if (ret)
goto out;
ret = __ufshcd_tx_eqtr(hba, params, pwr_mode);
if (ret)
goto out;
ret = ufshcd_vops_tx_eqtr_notify(hba, POST_CHANGE, pwr_mode);
out:
if (ret)
ufshcd_tx_eqtr_unprepare(hba, &old_pwr_info);
return ret;
}
int ufshcd_config_tx_eq_settings(struct ufs_hba *hba,
struct ufs_pa_layer_attr *pwr_mode,
bool force_tx_eqtr)
{
struct ufshcd_tx_eq_params *params;
u32 gear, rate;
if (!ufshcd_is_tx_eq_supported(hba) || !use_adaptive_txeq)
return 0;
if (!hba->max_pwr_info.is_valid) {
dev_err(hba->dev, "Max power info is invalid\n");
return -EINVAL;
}
if (!pwr_mode) {
dev_err(hba->dev, "Target power mode is NULL\n");
return -EINVAL;
}
gear = pwr_mode->gear_tx;
rate = pwr_mode->hs_rate;
if (gear < UFS_HS_G1 || gear > UFS_HS_GEAR_MAX) {
dev_err(hba->dev, "Invalid HS-Gear (%u) for TX Equalization\n",
gear);
return -EINVAL;
} else if (gear < max_t(u32, adaptive_txeq_gear, UFS_HS_G4)) {
return 0;
}
if (rate != PA_HS_MODE_A && rate != PA_HS_MODE_B) {
dev_err(hba->dev, "Invalid HS-Rate (%u) for TX Equalization\n",
rate);
return -EINVAL;
}
params = &hba->tx_eq_params[gear - 1];
if (!params->is_valid || force_tx_eqtr) {
int ret;
ret = ufshcd_tx_eqtr(hba, params, pwr_mode);
if (ret) {
dev_err(hba->dev, "Failed to train TX Equalization for HS-G%u, Rate-%s: %d\n",
gear, ufs_hs_rate_to_str(rate), ret);
return ret;
}
params->is_valid = true;
params->is_trained = true;
params->is_applied = false;
}
if (params->is_valid && !params->is_applied) {
int ret;
ret = ufshcd_apply_tx_eq_settings(hba, params, gear);
if (ret) {
dev_err(hba->dev, "Failed to apply TX Equalization settings for HS-G%u, Rate-%s: %d\n",
gear, ufs_hs_rate_to_str(rate), ret);
return ret;
}
params->is_applied = true;
}
return 0;
}
void ufshcd_apply_valid_tx_eq_settings(struct ufs_hba *hba)
{
struct ufshcd_tx_eq_params *params;
int gear, err;
if (!ufshcd_is_tx_eq_supported(hba))
return;
if (!hba->max_pwr_info.is_valid) {
dev_err(hba->dev, "Max power info is invalid, cannot apply TX Equalization settings\n");
return;
}
for (gear = UFS_HS_G1; gear <= UFS_HS_GEAR_MAX; gear++) {
params = &hba->tx_eq_params[gear - 1];
if (params->is_valid) {
err = ufshcd_apply_tx_eq_settings(hba, params, gear);
if (err) {
params->is_applied = false;
dev_err(hba->dev, "Failed to apply TX Equalization settings for HS-G%u: %d\n",
gear, err);
} else {
params->is_applied = true;
}
}
}
}
int ufshcd_retrain_tx_eq(struct ufs_hba *hba, u32 gear)
{
struct ufs_pa_layer_attr new_pwr_info, final_params = {};
int ret;
if (!ufshcd_is_tx_eq_supported(hba) || !use_adaptive_txeq)
return -EOPNOTSUPP;
if (gear < adaptive_txeq_gear)
return -ERANGE;
ufshcd_hold(hba);
ret = ufshcd_pause_command_processing(hba, 1 * USEC_PER_SEC);
if (ret) {
ufshcd_release(hba);
return ret;
}
if (ufshcd_is_clkscaling_supported(hba))
ufshcd_scale_clks(hba, ULONG_MAX, true);
new_pwr_info = hba->pwr_info;
new_pwr_info.gear_tx = gear;
new_pwr_info.gear_rx = gear;
ret = ufshcd_vops_negotiate_pwr_mode(hba, &new_pwr_info, &final_params);
if (ret)
memcpy(&final_params, &new_pwr_info, sizeof(final_params));
if (final_params.gear_tx != gear) {
dev_err(hba->dev, "Negotiated Gear (%u) does not match target Gear (%u)\n",
final_params.gear_tx, gear);
ret = -EINVAL;
goto out;
}
ret = ufshcd_config_tx_eq_settings(hba, &final_params, true);
if (ret) {
dev_err(hba->dev, "Failed to config TX Equalization for HS-G%u, Rate-%s: %d\n",
final_params.gear_tx,
ufs_hs_rate_to_str(final_params.hs_rate), ret);
goto out;
}
ret = ufshcd_change_power_mode(hba, &final_params,
UFSHCD_PMC_POLICY_FORCE);
if (ret)
dev_err(hba->dev, "%s: Failed to change Power Mode to HS-G%u, Rate-%s: %d\n",
__func__, final_params.gear_tx,
ufs_hs_rate_to_str(final_params.hs_rate), ret);
out:
ufshcd_resume_command_processing(hba);
ufshcd_release(hba);
return ret;
}
static void ufshcd_extract_tx_eq_settings_attrs(struct ufs_hba *hba, u8 gear)
{
struct ufshcd_tx_eq_params *params;
u32 lane, eq_ext;
int ret;
u64 eq;
ret = ufshcd_query_attr(hba, UPIU_QUERY_OPCODE_READ_ATTR,
QUERY_ATTR_IDN_TX_EQ_GN_SETTINGS_EXT, gear - 1,
(u8)txeq_setting_sel, &eq_ext);
if (ret)
return;
dev_dbg(hba->dev, "%s: HS-G%u wTxEQGnSettingsExt (Selector %u) = 0x%08x\n",
__func__, gear, txeq_setting_sel, eq_ext);
if (!(eq_ext & TX_EQ_SETTINGS_VALID_BIT))
return;
ret = ufshcd_query_attr_qword(hba, UPIU_QUERY_OPCODE_READ_ATTR,
QUERY_ATTR_IDN_TX_EQ_GN_SETTINGS,
gear - 1, (u8)txeq_setting_sel, &eq);
if (ret)
return;
dev_dbg(hba->dev, "%s: HS-G%u qTxEQGnSettings (Selector %u) = 0x%016llx\n",
__func__, gear, txeq_setting_sel, eq);
params = &hba->tx_eq_params[gear - 1];
for (lane = 0; lane < UFS_MAX_LANES; lane++) {
params->host[lane].preshoot = tx_eq_host_preshoot_decode(eq, lane);
params->host[lane].deemphasis = tx_eq_host_deemphasis_decode(eq, lane);
params->host[lane].precode_en = tx_eq_host_precode_en_decode(eq_ext, lane);
params->device[lane].preshoot = tx_eq_device_preshoot_decode(eq, lane);
params->device[lane].deemphasis = tx_eq_device_deemphasis_decode(eq, lane);
params->device[lane].precode_en = tx_eq_device_precode_en_decode(eq_ext, lane);
}
params->is_valid = true;
}
void ufshcd_retrieve_tx_eq_settings(struct ufs_hba *hba)
{
u8 gear = (u8)adaptive_txeq_gear;
if (!hba->max_pwr_info.is_valid || !ufshcd_is_tx_eq_supported(hba) ||
!use_adaptive_txeq || !retrieve_txeq_setting)
return;
for (; gear <= UFS_HS_GEAR_MAX; gear++)
ufshcd_extract_tx_eq_settings_attrs(hba, gear);
}
static void ufshcd_update_tx_eq_settings_attrs(struct ufs_hba *hba, u8 gear)
{
struct ufshcd_tx_eq_params *params;
u32 lane, eq_ext = 0;
u64 eq = 0;
int ret;
params = &hba->tx_eq_params[gear - 1];
if (!params->is_valid || !params->is_trained)
return;
for (lane = 0; lane < UFS_MAX_LANES; lane++) {
eq |= tx_eq_host_preshoot_encode((u64)params->host[lane].preshoot, lane);
eq |= tx_eq_host_deemphasis_encode((u64)params->host[lane].deemphasis, lane);
eq_ext |= tx_eq_host_precode_en_encode(params->host[lane].precode_en, lane);
eq |= tx_eq_device_preshoot_encode((u64)params->device[lane].preshoot, lane);
eq |= tx_eq_device_deemphasis_encode((u64)params->device[lane].deemphasis, lane);
eq_ext |= tx_eq_device_precode_en_encode(params->device[lane].precode_en, lane);
}
eq_ext |= TX_EQ_SETTINGS_VALID_BIT;
ret = ufshcd_query_attr_qword(hba, UPIU_QUERY_OPCODE_WRITE_ATTR,
QUERY_ATTR_IDN_TX_EQ_GN_SETTINGS,
gear - 1, (u8)txeq_setting_sel, &eq);
if (ret)
return;
ret = ufshcd_query_attr(hba, UPIU_QUERY_OPCODE_WRITE_ATTR,
QUERY_ATTR_IDN_TX_EQ_GN_SETTINGS_EXT, gear - 1,
(u8)txeq_setting_sel, &eq_ext);
if (ret)
return;
dev_dbg(hba->dev, "%s: Saved HS-G%u qTxEQGnSettings (Selector %u) = 0x%016llx\n",
__func__, gear, txeq_setting_sel, eq);
dev_dbg(hba->dev, "%s: Saved HS-G%u wTxEQGnSettingsExt (Selector %u) = 0x%08x\n",
__func__, gear, txeq_setting_sel, eq_ext);
}
void ufshcd_store_tx_eq_settings(struct ufs_hba *hba)
{
u8 gear = (u8)adaptive_txeq_gear;
if (!hba->max_pwr_info.is_valid || !ufshcd_is_tx_eq_supported(hba) ||
!use_adaptive_txeq || !store_txeq_setting)
return;
for (; gear <= UFS_HS_GEAR_MAX; gear++)
ufshcd_update_tx_eq_settings_attrs(hba, gear);
}
