@@ -10375,6 +10375,56 @@ static const struct blk_mq_ops ufshcd_tmf_ops = {
.queue_rq = ufshcd_queue_tmf,
};
+static int ufshcd_add_scsi_host(struct ufs_hba *hba)
+{
+ int err;
+
+ if (!is_mcq_supported(hba)) {
+ err = scsi_add_host(hba->host, hba->dev);
+ if (err) {
+ dev_err(hba->dev, "scsi_add_host failed\n");
+ return err;
+ }
+ hba->scsi_host_added = true;
+ }
+
+ hba->tmf_tag_set = (struct blk_mq_tag_set) {
+ .nr_hw_queues = 1,
+ .queue_depth = hba->nutmrs,
+ .ops = &ufshcd_tmf_ops,
+ .flags = BLK_MQ_F_NO_SCHED,
+ };
+ err = blk_mq_alloc_tag_set(&hba->tmf_tag_set);
+ if (err < 0)
+ goto remove_scsi_host;
+ hba->tmf_queue = blk_mq_alloc_queue(&hba->tmf_tag_set, NULL, NULL);
+ if (IS_ERR(hba->tmf_queue)) {
+ err = PTR_ERR(hba->tmf_queue);
+ goto free_tmf_tag_set;
+ }
+ hba->tmf_rqs = devm_kcalloc(hba->dev, hba->nutmrs,
+ sizeof(*hba->tmf_rqs), GFP_KERNEL);
+ if (!hba->tmf_rqs) {
+ err = -ENOMEM;
+ goto free_tmf_queue;
+ }
+
+ return 0;
+
+free_tmf_queue:
+ blk_mq_destroy_queue(hba->tmf_queue);
+ blk_put_queue(hba->tmf_queue);
+
+free_tmf_tag_set:
+ blk_mq_free_tag_set(&hba->tmf_tag_set);
+
+remove_scsi_host:
+ if (hba->scsi_host_added)
+ scsi_remove_host(hba->host);
+
+ return err;
+}
+
/**
* ufshcd_init - Driver initialization routine
* @hba: per-adapter instance
@@ -10506,35 +10556,9 @@ int ufshcd_init(struct ufs_hba *hba, void __iomem *mmio_base, unsigned int irq)
hba->is_irq_enabled = true;
}
- if (!is_mcq_supported(hba)) {
- err = scsi_add_host(host, hba->dev);
- if (err) {
- dev_err(hba->dev, "scsi_add_host failed\n");
- goto out_disable;
- }
- hba->scsi_host_added = true;
- }
-
- hba->tmf_tag_set = (struct blk_mq_tag_set) {
- .nr_hw_queues = 1,
- .queue_depth = hba->nutmrs,
- .ops = &ufshcd_tmf_ops,
- .flags = BLK_MQ_F_NO_SCHED,
- };
- err = blk_mq_alloc_tag_set(&hba->tmf_tag_set);
- if (err < 0)
- goto out_remove_scsi_host;
- hba->tmf_queue = blk_mq_alloc_queue(&hba->tmf_tag_set, NULL, NULL);
- if (IS_ERR(hba->tmf_queue)) {
- err = PTR_ERR(hba->tmf_queue);
- goto free_tmf_tag_set;
- }
- hba->tmf_rqs = devm_kcalloc(hba->dev, hba->nutmrs,
- sizeof(*hba->tmf_rqs), GFP_KERNEL);
- if (!hba->tmf_rqs) {
- err = -ENOMEM;
- goto free_tmf_queue;
- }
+ err = ufshcd_add_scsi_host(hba);
+ if (err)
+ goto out_disable;
/* Reset the attached device */
ufshcd_device_reset(hba);
@@ -10592,9 +10616,7 @@ int ufshcd_init(struct ufs_hba *hba, void __iomem *mmio_base, unsigned int irq)
free_tmf_queue:
blk_mq_destroy_queue(hba->tmf_queue);
blk_put_queue(hba->tmf_queue);
-free_tmf_tag_set:
blk_mq_free_tag_set(&hba->tmf_tag_set);
-out_remove_scsi_host:
if (hba->scsi_host_added)
scsi_remove_host(hba->host);
out_disable: