[PATCH 2/2] scsi: ufs: Workaround UFS devices that object to DeepSleep IMMED

[Adrian Hunter <adrian.hunter@intel.com>]

The UFS specification says to set the IMMED (immediate) flag for the
Start/Stop Unit command when entering DeepSleep. However some UFS
devices object to that. Workaround that by retrying without IMMED.
Whichever possibility works, the result is recorded for the next
time.

Signed-off-by: Adrian Hunter <adrian.hunter@intel.com>
---
 drivers/scsi/ufs/ufshcd.c | 53 +++++++++++++++++++++++++++++++++------
 drivers/scsi/ufs/ufshcd.h | 11 ++++++++
 2 files changed, 56 insertions(+), 8 deletions(-)

diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index d072b0c80bd8..3a67a711c0ae 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -8202,6 +8202,44 @@ ufshcd_send_request_sense(struct ufs_hba *hba, struct scsi_device *sdp)
 	return ret;
 }
 
+static bool ufshcd_set_immed(struct ufs_hba *hba,
+			     enum ufs_dev_pwr_mode pwr_mode)
+{
+	/*
+	 * DeepSleep requires the Immediate flag. DeepSleep state is actually
+	 * entered when the link state goes to Hibern8.
+	 */
+	return pwr_mode == UFS_DEEPSLEEP_PWR_MODE &&
+	       hba->deepsleep_immed != UFS_DEEPSLEEP_IMMED_BROKEN;
+}
+
+static bool ufshcd_retry_dev_pwr_mode(struct ufs_hba *hba,
+				      enum ufs_dev_pwr_mode pwr_mode,
+				      unsigned char *cmd, int ret,
+				      struct scsi_sense_hdr *sshdr)
+{
+	if (pwr_mode == UFS_DEEPSLEEP_PWR_MODE &&
+	    hba->deepsleep_immed == UFS_DEEPSLEEP_IMMED_UNKNOWN &&
+	    (cmd[1] & 1) && driver_byte(ret) == DRIVER_SENSE &&
+	    scsi_sense_valid(sshdr) && sshdr->sense_key == ILLEGAL_REQUEST) {
+		cmd[1] &= ~1;
+		return true;
+	}
+	return false;
+}
+
+static void ufshcd_set_dev_pwr_mode_success(struct ufs_hba *hba,
+					    enum ufs_dev_pwr_mode pwr_mode,
+					    unsigned char *cmd)
+{
+	if (pwr_mode == UFS_DEEPSLEEP_PWR_MODE &&
+	    hba->deepsleep_immed == UFS_DEEPSLEEP_IMMED_UNKNOWN) {
+		hba->deepsleep_immed = (cmd[1] & 1) ?
+					UFS_DEEPSLEEP_IMMED_OK :
+					UFS_DEEPSLEEP_IMMED_BROKEN;
+	}
+}
+
 /**
  * ufshcd_set_dev_pwr_mode - sends START STOP UNIT command to set device
  *			     power mode
@@ -8251,14 +8289,9 @@ static int ufshcd_set_dev_pwr_mode(struct ufs_hba *hba,
 		hba->wlun_dev_clr_ua = false;
 	}
 
-	/*
-	 * DeepSleep requires the Immediate flag. DeepSleep state is actually
-	 * entered when the link state goes to Hibern8.
-	 */
-	if (pwr_mode == UFS_DEEPSLEEP_PWR_MODE)
-		cmd[1] = 1;
+	cmd[1] = ufshcd_set_immed(hba, pwr_mode) ? 1 : 0;
 	cmd[4] = pwr_mode << 4;
-
+retry:
 	/*
 	 * Current function would be generally called from the power management
 	 * callbacks hence set the RQF_PM flag so that it doesn't resume the
@@ -8267,6 +8300,8 @@ static int ufshcd_set_dev_pwr_mode(struct ufs_hba *hba,
 	ret = scsi_execute(sdp, cmd, DMA_NONE, NULL, 0, NULL, &sshdr,
 			START_STOP_TIMEOUT, 0, 0, RQF_PM, NULL);
 	if (ret) {
+		if (ufshcd_retry_dev_pwr_mode(hba, pwr_mode, cmd, ret, &sshdr))
+			goto retry;
 		sdev_printk(KERN_WARNING, sdp,
 			    "START_STOP failed for power mode: %d, result %x\n",
 			    pwr_mode, ret);
@@ -8274,8 +8309,10 @@ static int ufshcd_set_dev_pwr_mode(struct ufs_hba *hba,
 			scsi_print_sense_hdr(sdp, NULL, &sshdr);
 	}
 
-	if (!ret)
+	if (!ret) {
 		hba->curr_dev_pwr_mode = pwr_mode;
+		ufshcd_set_dev_pwr_mode_success(hba, pwr_mode, cmd);
+	}
 out:
 	scsi_device_put(sdp);
 	hba->host->eh_noresume = 0;
diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index 8c6094fb35f4..b4bf00891c9f 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h
@@ -147,6 +147,16 @@ struct ufs_pm_lvl_states {
 	enum uic_link_state link_state;
 };
 
+/*
+ * Whether or not to set the immediate flag for the DeepSleep START_STOP unit
+ * command.
+ */
+enum ufs_deepsleep_immed {
+	UFS_DEEPSLEEP_IMMED_UNKNOWN,	/* Set IMMED, but retry without it */
+	UFS_DEEPSLEEP_IMMED_OK,		/* Set IMMED */
+	UFS_DEEPSLEEP_IMMED_BROKEN,	/* Do not set IMMED */
+};
+
 /**
  * struct ufshcd_lrb - local reference block
  * @utr_descriptor_ptr: UTRD address of the command
@@ -705,6 +715,7 @@ struct ufs_hba {
 	struct device_attribute rpm_lvl_attr;
 	struct device_attribute spm_lvl_attr;
 	int pm_op_in_progress;
+	enum ufs_deepsleep_immed deepsleep_immed;
 
 	/* Auto-Hibernate Idle Timer register value */
 	u32 ahit;
-- 
2.17.1