diff --git a/drivers/usb/phy/phy-msm-ssusb-qmp.c b/drivers/usb/phy/phy-msm-ssusb-qmp.c index ecb548624ed1..b220c72b711a 100644 --- a/drivers/usb/phy/phy-msm-ssusb-qmp.c +++ b/drivers/usb/phy/phy-msm-ssusb-qmp.c @@ -365,6 +365,44 @@ static void msm_ssphy_qmp_setmode(struct msm_ssphy_qmp *phy, u32 mode) readl_relaxed(phy->base + phy->phy_reg[USB3_DP_COM_PHY_MODE_CTRL]); } +static void usb_qmp_update_hw_portselect(struct msm_ssphy_qmp *phy) +{ + struct pinctrl *portselect_pinctrl; + struct pinctrl_state *portselect_state; + + if (phy->phy.dev->pins) { + portselect_pinctrl = phy->phy.dev->pins->p; + portselect_state = phy->phy.dev->pins->default_state; + } else { + portselect_pinctrl = pinctrl_get(phy->phy.dev); + if (IS_ERR_OR_NULL(portselect_pinctrl)) { + dev_dbg(phy->phy.dev, "failed to get pinctrl\n"); + return; + } + + portselect_state = + pinctrl_lookup_state(portselect_pinctrl, "portselect"); + if (IS_ERR_OR_NULL(portselect_state)) { + dev_dbg(phy->phy.dev, + "failed to find portselect state\n"); + pinctrl_put(portselect_pinctrl); + return; + } + } + + dev_dbg(phy->phy.dev, "use hw port select\n"); + + writel_relaxed(0x01, + phy->base + phy->phy_reg[USB3_DP_COM_SW_RESET]); + + pinctrl_select_state(portselect_pinctrl, portselect_state); + + writel_relaxed(0x00, + phy->base + phy->phy_reg[USB3_DP_COM_SW_RESET]); + + if (!phy->phy.dev->pins) + pinctrl_put(portselect_pinctrl); +} static void usb_qmp_update_portselect_phymode(struct msm_ssphy_qmp *phy) { @@ -382,16 +420,7 @@ static void usb_qmp_update_portselect_phymode(struct msm_ssphy_qmp *phy) switch (phy->phy_type) { case USB3_AND_DP: - if (phy->phy.dev->pins) { - writel_relaxed(0x01, - phy->base + phy->phy_reg[USB3_DP_COM_SW_RESET]); - - pinctrl_select_state(phy->phy.dev->pins->p, - phy->phy.dev->pins->default_state); - - writel_relaxed(0x00, - phy->base + phy->phy_reg[USB3_DP_COM_SW_RESET]); - } + usb_qmp_update_hw_portselect(phy); /* override hardware control for reset of qmp phy */ writel_relaxed(SW_DPPHY_RESET_MUX | SW_DPPHY_RESET |