--- zzzz-none-000/linux-4.4.271/drivers/usb/chipidea/ci_hdrc_msm.c	2021-06-03 06:22:09.000000000 +0000
+++ hawkeye-5590-750/linux-4.4.271/drivers/usb/chipidea/ci_hdrc_msm.c	2023-04-19 10:22:29.000000000 +0000
@@ -12,10 +12,20 @@
 #include <linux/usb/ulpi.h>
 #include <linux/usb/gadget.h>
 #include <linux/usb/chipidea.h>
+#include <linux/clk.h>
+#include <linux/reset.h>
 
 #include "ci.h"
 
 #define MSM_USB_BASE	(ci->hw_bank.abs)
+#define USB_PHY_CLK_SOURCE_CLK 0x000CBC7A
+
+struct ci_hdrc_msm {
+	struct platform_device *ci;
+	struct clk *core_clk;
+	struct clk *iface_clk;
+	struct clk *fs_clk;
+};
 
 static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 {
@@ -26,6 +36,11 @@
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_RESET_EVENT received\n");
 		writel(0, USB_AHBBURST);
 		writel(0, USB_AHBMODE);
+		/*
+		 * Controller reset, resets clock source configuration.
+		 * Reconfigure clock source.
+		 */
+		writel(USB_PHY_CLK_SOURCE_CLK, USB_PHY_CTRL);
 		usb_phy_init(ci->usb_phy);
 		break;
 	case CI_HDRC_CONTROLLER_STOPPED_EVENT:
@@ -53,11 +68,20 @@
 
 static int ci_hdrc_msm_probe(struct platform_device *pdev)
 {
+	struct ci_hdrc_msm *ci;
 	struct platform_device *plat_ci;
 	struct usb_phy *phy;
+	struct clk *clk;
+	struct reset_control *reset;
+	int ret;
 
 	dev_dbg(&pdev->dev, "ci_hdrc_msm_probe\n");
 
+	ci = devm_kzalloc(&pdev->dev, sizeof(*ci), GFP_KERNEL);
+	if (!ci)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, ci);
+
 	/*
 	 * OTG(PHY) driver takes care of PHY initialization, clock management,
 	 * powering up VBUS, mapping of registers address space and power
@@ -69,28 +93,85 @@
 
 	ci_hdrc_msm_platdata.usb_phy = phy;
 
+	reset = devm_reset_control_get(&pdev->dev, "core");
+	if (IS_ERR(reset))
+		return PTR_ERR(reset);
+
+	ci->core_clk = clk = devm_clk_get(&pdev->dev, "core");
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	ci->iface_clk = clk = devm_clk_get(&pdev->dev, "iface");
+	if (IS_ERR(clk)) {
+		dev_info(&pdev->dev, "No iface clock found\n");
+		ci->iface_clk = NULL;
+	}
+
+	ci->fs_clk = clk = devm_clk_get(&pdev->dev, "fs");
+	if (IS_ERR(clk)) {
+		if (PTR_ERR(clk) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		ci->fs_clk = NULL;
+	}
+
+	if (ci->fs_clk) {
+		ret = clk_prepare_enable(ci->fs_clk);
+		if (ret)
+			return ret;
+	}
+
+	reset_control_assert(reset);
+	usleep_range(10000, 12000);
+	reset_control_deassert(reset);
+
+	if (ci->fs_clk)
+		clk_disable_unprepare(ci->fs_clk);
+
+	ret = clk_prepare_enable(ci->core_clk);
+	if (ret)
+		return ret;
+
+	if (ci->iface_clk) {
+		ret = clk_prepare_enable(ci->iface_clk);
+		if (ret)
+			goto err_iface;
+	}
+
 	plat_ci = ci_hdrc_add_device(&pdev->dev,
 				pdev->resource, pdev->num_resources,
 				&ci_hdrc_msm_platdata);
 	if (IS_ERR(plat_ci)) {
 		dev_err(&pdev->dev, "ci_hdrc_add_device failed!\n");
-		return PTR_ERR(plat_ci);
+		ret = PTR_ERR(plat_ci);
+		goto err_mux;
 	}
 
-	platform_set_drvdata(pdev, plat_ci);
+	ci->ci = plat_ci;
 
 	pm_runtime_no_callbacks(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
 	return 0;
+
+err_mux:
+	if (ci->iface_clk)
+		clk_disable_unprepare(ci->iface_clk);
+err_iface:
+	clk_disable_unprepare(ci->core_clk);
+	return ret;
 }
 
 static int ci_hdrc_msm_remove(struct platform_device *pdev)
 {
-	struct platform_device *plat_ci = platform_get_drvdata(pdev);
+	struct ci_hdrc_msm *ci = platform_get_drvdata(pdev);
 
 	pm_runtime_disable(&pdev->dev);
-	ci_hdrc_remove_device(plat_ci);
+	ci_hdrc_remove_device(ci->ci);
+
+	if (ci->iface_clk)
+		clk_disable_unprepare(ci->iface_clk);
+
+	clk_disable_unprepare(ci->core_clk);
 
 	return 0;
 }