[RFC PATCH 04/13] scsi: ufs: ufshpb: Init part II - Attach scsi device

[Avri Altman <avri.altman@wdc.com>]

The ufs boot process is essentially comprised of 2 parts: first a
handshake with the device, and then, scsi scans and assign a scsi device
to each lun.  The latter, although running a-synchronically, is
happening right after reading the device configuration - lun by lun.

By now we've read the device HPB configuration, and we are ready  to
attach a scsi device to our HPB luns.  A perfect timing might be while
scsi is performing its .slave_alloc() or .slave_configure().

Signed-off-by: Avri Altman <avri.altman@wdc.com>
---
 drivers/scsi/ufs/ufshcd.c |   3 ++
 drivers/scsi/ufs/ufshpb.c | 103 ++++++++++++++++++++++++++++++++++++++++++++++
 drivers/scsi/ufs/ufshpb.h |   3 ++
 3 files changed, 109 insertions(+)

diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index bffe699..c2011bf 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -4628,6 +4628,9 @@ static int ufshcd_slave_alloc(struct scsi_device *sdev)
 
 	ufshcd_get_lu_power_on_wp_status(hba, sdev);
 
+	if (ufshcd_is_hpb_supported(hba))
+		ufshpb_attach_sdev(hba, sdev);
+
 	return 0;
 }
 
diff --git a/drivers/scsi/ufs/ufshpb.c b/drivers/scsi/ufs/ufshpb.c
index e94e699..4a10e7b 100644
--- a/drivers/scsi/ufs/ufshpb.c
+++ b/drivers/scsi/ufs/ufshpb.c
@@ -7,7 +7,9 @@
  */
 
 #include <asm/unaligned.h>
+#include <scsi/scsi_dh.h>
 #include <scsi/scsi_dh_ufshpb.h>
+#include "../scsi_priv.h"
 #include "ufshcd.h"
 #include "ufs.h"
 #include "ufshpb.h"
@@ -25,6 +27,7 @@ enum ufshpb_control_modes {
 
 struct ufshpb_lun {
 	u8 lun;
+	struct scsi_device *sdev;
 };
 
 
@@ -34,6 +37,91 @@ struct ufshpb_lun *ufshpb_luns;
 static unsigned long ufshpb_lun_map[BITS_TO_LONGS(UFSHPB_MAX_LUNS)];
 static u8 ufshpb_lun_lookup[UFSHPB_MAX_LUNS];
 
+void ufshpb_remove_lun(u8 lun)
+{
+	struct ufshpb_lun *hpb;
+
+	if (!ufshpb_luns)
+		return;
+
+	hpb = ufshpb_luns + lun;
+	if (hpb->sdev && hpb->sdev->handler_data) {
+		if (scsi_device_get(hpb->sdev))
+			return;
+
+		scsi_dh_release_device(hpb->sdev);
+		scsi_device_put(hpb->sdev);
+	}
+}
+
+/**
+ * ufshpb_attach_sdev - attach and activate a hpb device handler
+ * @hba: per adapter object
+ * @sdev: scsi device that owns that handler
+ *
+ * Called during .slave_alloc(), and after ufshpb_probe() read the device hpb
+ * configuration.
+ */
+void ufshpb_attach_sdev(struct ufs_hba *hba, struct scsi_device *sdev)
+{
+	struct ufshpb_lun *hpb_lun;
+	struct ufshpb_dh_data {
+		struct ufshpb_config *c;
+		struct ufshpb_lun_config *lc;
+	} h;
+	int ret = -EINVAL;
+	u8 lun = sdev->lun;
+	u8 i;
+
+	/* ignore w-luns as those can't be hpb luns */
+	if (lun >= UFSHPB_MAX_LUNS)
+		return;
+
+	/* ignore non-hpb luns */
+	if (!test_bit(lun, ufshpb_lun_map))
+		return;
+
+	i = ufshpb_lun_lookup[lun];
+
+	if (sdev->handler && sdev->handler_data) {
+		dev_err(hba->dev, "trying to re-attach lun %d ?\n", lun);
+		goto out;
+	}
+
+	if (!ufshpb_luns) {
+		dev_err(hba->dev, "HPB was already removed\n");
+		goto out;
+	}
+
+	if (scsi_device_get(sdev)) {
+		dev_err(hba->dev, "failed to get sdev for lun %d\n", lun);
+		goto out;
+	}
+
+	hpb_lun = ufshpb_luns + i;
+	hpb_lun->sdev = sdev;
+
+	ret = scsi_dh_attach(sdev->request_queue, "ufshpb");
+	if (ret || !sdev->handler || !sdev->handler_data)
+		goto put_scsi_dev;
+
+	h.c = ufshpb_conf;
+	h.lc = ufshpb_luns_conf + i;
+
+	memcpy(sdev->handler_data, &h, sizeof(h));
+
+	ret = scsi_dh_activate(sdev->request_queue, NULL, NULL);
+
+put_scsi_dev:
+	scsi_device_put(sdev);
+
+out:
+	if (ret) {
+		dev_err(hba->dev, "attach sdev to HPB lun %d failed\n", lun);
+		ufshpb_remove_lun(i);
+	}
+}
+
 /**
  * ufshpb_remove - ufshpb cleanup
  *
@@ -41,9 +129,24 @@ static u8 ufshpb_lun_lookup[UFSHPB_MAX_LUNS];
  */
 void ufshpb_remove(struct ufs_hba *hba)
 {
+	if (!ufshpb_conf)
+		goto remove_hpb;
+
+	if (ufshpb_luns) {
+		unsigned int num_hpb_luns = ufshpb_conf->num_hpb_luns;
+		int i;
+
+		spin_lock(hba->host->host_lock);
+		for (i = 0; i < num_hpb_luns; i++)
+			ufshpb_remove_lun(i);
+		spin_unlock(hba->host->host_lock);
+	}
+
 	kfree(ufshpb_conf);
 	kfree(ufshpb_luns_conf);
 	kfree(ufshpb_luns);
+
+remove_hpb:
 	ufshcd_query_flag(hba, UPIU_QUERY_OPCODE_SET_FLAG,
 			  QUERY_FLAG_IDN_HPB_RESET, 0, NULL);
 }
diff --git a/drivers/scsi/ufs/ufshpb.h b/drivers/scsi/ufs/ufshpb.h
index ee990f4..276a749 100644
--- a/drivers/scsi/ufs/ufshpb.h
+++ b/drivers/scsi/ufs/ufshpb.h
@@ -14,9 +14,12 @@ struct ufs_hba;
 #ifdef CONFIG_SCSI_UFS_HPB
 void ufshpb_remove(struct ufs_hba *hba);
 int ufshpb_probe(struct ufs_hba *hba);
+void ufshpb_attach_sdev(struct ufs_hba *hba, struct scsi_device *sdev);
 #else
 static inline void ufshpb_remove(struct ufs_hba *hba) {}
 static inline int ufshpb_probe(struct ufs_hba *hba) { return 0; }
+static inline void
+ufshpb_attach_sdev(struct ufs_hba *hba, struct scsi_device *sdev) {}
 #endif
 
 #endif /* _UFSHPB_H */
-- 
2.7.4