1 files changed, 36 insertions, 39 deletions

diff --git a/drivers/ufs/host/ufs-qcom.c b/drivers/ufs/host/ufs-qcom.c
index 6913dda3a744..1b37449fbffc 100644
--- a/drivers/ufs/host/ufs-qcom.c
+++ b/drivers/ufs/host/ufs-qcom.c
@@ -1806,19 +1806,15 @@ static void ufs_qcom_write_msi_msg(struct msi_desc *desc, struct msi_msg *msg)
 	ufshcd_mcq_config_esi(hba, msg);
 }
 
-struct ufs_qcom_irq {
-	unsigned int		irq;
-	unsigned int		idx;
-	struct ufs_hba		*hba;
-};
-
 static irqreturn_t ufs_qcom_mcq_esi_handler(int irq, void *data)
 {
-	struct ufs_qcom_irq *qi = data;
-	struct ufs_hba *hba = qi->hba;
-	struct ufs_hw_queue *hwq = &hba->uhq[qi->idx];
+	struct msi_desc *desc = data;
+	struct device *dev = msi_desc_to_dev(desc);
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+	u32 id = desc->msi_index;
+	struct ufs_hw_queue *hwq = &hba->uhq[id];
 
-	ufshcd_mcq_write_cqis(hba, 0x1, qi->idx);
+	ufshcd_mcq_write_cqis(hba, 0x1, id);
 	ufshcd_mcq_poll_cqe_lock(hba, hwq);
 
 	return IRQ_HANDLED;
@@ -1827,7 +1823,8 @@ static irqreturn_t ufs_qcom_mcq_esi_handler(int irq, void *data)
 static int ufs_qcom_config_esi(struct ufs_hba *hba)
 {
 	struct ufs_qcom_host *host = ufshcd_get_variant(hba);
-	struct ufs_qcom_irq *qi;
+	struct msi_desc *desc;
+	struct msi_desc *failed_desc = NULL;
 	int nr_irqs, ret;
 
 	if (host->esi_enabled)
@@ -1838,47 +1835,47 @@ static int ufs_qcom_config_esi(struct ufs_hba *hba)
 	 * 2. Poll queues do not need ESI.
 	 */
 	nr_irqs = hba->nr_hw_queues - hba->nr_queues[HCTX_TYPE_POLL];
-	qi = devm_kcalloc(hba->dev, nr_irqs, sizeof(*qi), GFP_KERNEL);
-	if (qi)
-		return -ENOMEM;
-
 	ret = platform_device_msi_init_and_alloc_irqs(hba->dev, nr_irqs,
 						      ufs_qcom_write_msi_msg);
 	if (ret) {
 		dev_err(hba->dev, "Failed to request Platform MSI %d\n", ret);
-		goto cleanup;
+		return ret;
 	}
 
-	for (int idx = 0; idx < nr_irqs; idx++) {
-		qi[idx].irq = msi_get_virq(hba->dev, idx);
-		qi[idx].idx = idx;
-		qi[idx].hba = hba;
-
-		ret = devm_request_irq(hba->dev, qi[idx].irq, ufs_qcom_mcq_esi_handler,
-				       IRQF_SHARED, "qcom-mcq-esi", qi + idx);
+	msi_lock_descs(hba->dev);
+	msi_for_each_desc(desc, hba->dev, MSI_DESC_ALL) {
+		ret = devm_request_irq(hba->dev, desc->irq,
+				       ufs_qcom_mcq_esi_handler,
+				       IRQF_SHARED, "qcom-mcq-esi", desc);
 		if (ret) {
 			dev_err(hba->dev, "%s: Fail to request IRQ for %d, err = %d\n",
-				__func__, qi[idx].irq, ret);
-			qi[idx].irq = 0;
-			goto cleanup;
+				__func__, desc->irq, ret);
+			failed_desc = desc;
+			break;
 		}
 	}
+	msi_unlock_descs(hba->dev);
 
-	if (host->hw_ver.major == 6 && host->hw_ver.minor == 0 &&
-	    host->hw_ver.step == 0) {
-		ufshcd_rmwl(hba, ESI_VEC_MASK,
-			    FIELD_PREP(ESI_VEC_MASK, MAX_ESI_VEC - 1),
-			    REG_UFS_CFG3);
+	if (ret) {
+		/* Rewind */
+		msi_lock_descs(hba->dev);
+		msi_for_each_desc(desc, hba->dev, MSI_DESC_ALL) {
+			if (desc == failed_desc)
+				break;
+			devm_free_irq(hba->dev, desc->irq, hba);
+		}
+		msi_unlock_descs(hba->dev);
+		platform_device_msi_free_irqs_all(hba->dev);
+	} else {
+		if (host->hw_ver.major == 6 && host->hw_ver.minor == 0 &&
+		    host->hw_ver.step == 0)
+			ufshcd_rmwl(hba, ESI_VEC_MASK,
+				    FIELD_PREP(ESI_VEC_MASK, MAX_ESI_VEC - 1),
+				    REG_UFS_CFG3);
+		ufshcd_mcq_enable_esi(hba);
+		host->esi_enabled = true;
 	}
-	ufshcd_mcq_enable_esi(hba);
-	host->esi_enabled = true;
-	return 0;
 
-cleanup:
-	for (int idx = 0; qi[idx].irq; idx++)
-		devm_free_irq(hba->dev, qi[idx].irq, hba);
-	platform_device_msi_free_irqs_all(hba->dev);
-	devm_kfree(hba->dev, qi);
 	return ret;
 }