forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2023-12-09 b22da3d8526a935aa31e086e63f60ff3246cb61c 
 kernel/drivers/memstick/core/mspro_block.c
....@@ -1,18 +1,14 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  *  Sony MemoryStick Pro storage support
34  *
45  *  Copyright (C) 2007 Alex Dubov <oakad@yahoo.com>
56  *
6
- * This program is free software; you can redistribute it and/or modify
7
- * it under the terms of the GNU General Public License version 2 as
8
- * published by the Free Software Foundation.
9
- *
107  * Special thanks to Carlos Corbacho for providing various MemoryStick cards
118  * that made this driver possible.
12
- *
139  */
1410 
15
-#include <linux/blkdev.h>
11
+#include <linux/blk-mq.h>
1612 #include <linux/idr.h>
1713 #include <linux/hdreg.h>
1814 #include <linux/kthread.h>
....@@ -142,6 +138,7 @@
142138 	struct gendisk        *disk;
143139 	struct request_queue  *queue;
144140 	struct request        *block_req;
141
+	struct blk_mq_tag_set tag_set;
145142 	spinlock_t            q_lock;
146143 
147144 	unsigned short        page_size;
....@@ -152,7 +149,6 @@
152149 	unsigned char         system;
153150 	unsigned char         read_only:1,
154151 			      eject:1,
155
-			      has_request:1,
156152 			      data_dir:1,
157153 			      active:1;
158154 	unsigned char         transfer_cmd;
....@@ -694,14 +690,13 @@
694690 
695691 /*** Data transfer ***/
696692 
697
-static int mspro_block_issue_req(struct memstick_dev *card, int chunk)
693
+static int mspro_block_issue_req(struct memstick_dev *card)
698694 {
699695 	struct mspro_block_data *msb = memstick_get_drvdata(card);
700696 	u64 t_off;
701697 	unsigned int count;
702698 
703
-try_again:
704
-	while (chunk) {
699
+	while (true) {
705700 		msb->current_page = 0;
706701 		msb->current_seg = 0;
707702 		msb->seg_count = blk_rq_map_sg(msb->block_req->q,
....@@ -709,9 +704,18 @@
709704 					       msb->req_sg);
710705 
711706 		if (!msb->seg_count) {
712
-			chunk = __blk_end_request_cur(msb->block_req,
713
-					BLK_STS_RESOURCE);
714
-			continue;
707
+			unsigned int bytes = blk_rq_cur_bytes(msb->block_req);
708
+			bool chunk;
709
+
710
+			chunk = blk_update_request(msb->block_req,
711
+							BLK_STS_RESOURCE,
712
+							bytes);
713
+			if (chunk)
714
+				continue;
715
+			__blk_mq_end_request(msb->block_req,
716
+						BLK_STS_RESOURCE);
717
+			msb->block_req = NULL;
718
+			return -EAGAIN;
715719 		}
716720 
717721 		t_off = blk_rq_pos(msb->block_req);
....@@ -728,31 +732,21 @@
728732 		memstick_new_req(card->host);
729733 		return 0;
730734 	}
731
-
732
-	dev_dbg(&card->dev, "blk_fetch\n");
733
-	msb->block_req = blk_fetch_request(msb->queue);
734
-	if (!msb->block_req) {
735
-		dev_dbg(&card->dev, "issue end\n");
736
-		return -EAGAIN;
737
-	}
738
-
739
-	dev_dbg(&card->dev, "trying again\n");
740
-	chunk = 1;
741
-	goto try_again;
742735 }
743736 
744737 static int mspro_block_complete_req(struct memstick_dev *card, int error)
745738 {
746739 	struct mspro_block_data *msb = memstick_get_drvdata(card);
747
-	int chunk, cnt;
740
+	int cnt;
741
+	bool chunk;
748742 	unsigned int t_len = 0;
749743 	unsigned long flags;
750744 
751745 	spin_lock_irqsave(&msb->q_lock, flags);
752
-	dev_dbg(&card->dev, "complete %d, %d\n", msb->has_request ? 1 : 0,
746
+	dev_dbg(&card->dev, "complete %d, %d\n", msb->block_req ? 1 : 0,
753747 		error);
754748 
755
-	if (msb->has_request) {
749
+	if (msb->block_req) {
756750 		/* Nothing to do - not really an error */
757751 		if (error == -EAGAIN)
758752 			error = 0;
....@@ -777,15 +771,17 @@
777771 		if (error && !t_len)
778772 			t_len = blk_rq_cur_bytes(msb->block_req);
779773 
780
-		chunk = __blk_end_request(msb->block_req,
774
+		chunk = blk_update_request(msb->block_req,
781775 				errno_to_blk_status(error), t_len);
782
-
783
-		error = mspro_block_issue_req(card, chunk);
784
-
785
-		if (!error)
786
-			goto out;
787
-		else
788
-			msb->has_request = 0;
776
+		if (chunk) {
777
+			error = mspro_block_issue_req(card);
778
+			if (!error)
779
+				goto out;
780
+		} else {
781
+			__blk_mq_end_request(msb->block_req,
782
+						errno_to_blk_status(error));
783
+			msb->block_req = NULL;
784
+		}
789785 	} else {
790786 		if (!error)
791787 			error = -EAGAIN;
....@@ -806,8 +802,8 @@
806802 
807803 	while (1) {
808804 		spin_lock_irqsave(&msb->q_lock, flags);
809
-		if (!msb->has_request) {
810
-			blk_stop_queue(msb->queue);
805
+		if (!msb->block_req) {
806
+			blk_mq_stop_hw_queues(msb->queue);
811807 			rc = 1;
812808 		}
813809 		spin_unlock_irqrestore(&msb->q_lock, flags);
....@@ -822,32 +818,37 @@
822818 static void mspro_block_start(struct memstick_dev *card)
823819 {
824820 	struct mspro_block_data *msb = memstick_get_drvdata(card);
825
-	unsigned long flags;
826821 
827
-	spin_lock_irqsave(&msb->q_lock, flags);
828
-	blk_start_queue(msb->queue);
829
-	spin_unlock_irqrestore(&msb->q_lock, flags);
822
+	blk_mq_start_hw_queues(msb->queue);
830823 }
831824 
832
-static void mspro_block_submit_req(struct request_queue *q)
825
+static blk_status_t mspro_queue_rq(struct blk_mq_hw_ctx *hctx,
826
+				   const struct blk_mq_queue_data *bd)
833827 {
834
-	struct memstick_dev *card = q->queuedata;
828
+	struct memstick_dev *card = hctx->queue->queuedata;
835829 	struct mspro_block_data *msb = memstick_get_drvdata(card);
836
-	struct request *req = NULL;
837830 
838
-	if (msb->has_request)
839
-		return;
831
+	spin_lock_irq(&msb->q_lock);
840832 
841
-	if (msb->eject) {
842
-		while ((req = blk_fetch_request(q)) != NULL)
843
-			__blk_end_request_all(req, BLK_STS_IOERR);
844
-
845
-		return;
833
+	if (msb->block_req) {
834
+		spin_unlock_irq(&msb->q_lock);
835
+		return BLK_STS_DEV_RESOURCE;
846836 	}
847837 
848
-	msb->has_request = 1;
849
-	if (mspro_block_issue_req(card, 0))
850
-		msb->has_request = 0;
838
+	if (msb->eject) {
839
+		spin_unlock_irq(&msb->q_lock);
840
+		blk_mq_start_request(bd->rq);
841
+		return BLK_STS_IOERR;
842
+	}
843
+
844
+	msb->block_req = bd->rq;
845
+	blk_mq_start_request(bd->rq);
846
+
847
+	if (mspro_block_issue_req(card))
848
+		msb->block_req = NULL;
849
+
850
+	spin_unlock_irq(&msb->q_lock);
851
+	return BLK_STS_OK;
851852 }
852853 
853854 /*** Initialization ***/
....@@ -1167,6 +1168,10 @@
11671168 
11681169 }
11691170 
1171
+static const struct blk_mq_ops mspro_mq_ops = {
1172
+	.queue_rq	= mspro_queue_rq,
1173
+};
1174
+
11701175 static int mspro_block_init_disk(struct memstick_dev *card)
11711176 {
11721177 	struct mspro_block_data *msb = memstick_get_drvdata(card);
....@@ -1206,9 +1211,11 @@
12061211 		goto out_release_id;
12071212 	}
12081213 
1209
-	msb->queue = blk_init_queue(mspro_block_submit_req, &msb->q_lock);
1210
-	if (!msb->queue) {
1211
-		rc = -ENOMEM;
1214
+	msb->queue = blk_mq_init_sq_queue(&msb->tag_set, &mspro_mq_ops, 2,
1215
+						BLK_MQ_F_SHOULD_MERGE);
1216
+	if (IS_ERR(msb->queue)) {
1217
+		rc = PTR_ERR(msb->queue);
1218
+		msb->queue = NULL;
12121219 		goto out_put_disk;
12131220 	}
12141221 
....@@ -1236,7 +1243,7 @@
12361243 	set_capacity(msb->disk, capacity);
12371244 	dev_dbg(&card->dev, "capacity set %ld\n", capacity);
12381245 
1239
-	device_add_disk(&card->dev, msb->disk);
1246
+	device_add_disk(&card->dev, msb->disk, NULL);
12401247 	msb->active = 1;
12411248 	return 0;
12421249 
....@@ -1318,13 +1325,14 @@
13181325 
13191326 	spin_lock_irqsave(&msb->q_lock, flags);
13201327 	msb->eject = 1;
1321
-	blk_start_queue(msb->queue);
13221328 	spin_unlock_irqrestore(&msb->q_lock, flags);
1329
+	blk_mq_start_hw_queues(msb->queue);
13231330 
13241331 	del_gendisk(msb->disk);
13251332 	dev_dbg(&card->dev, "mspro block remove\n");
13261333 
13271334 	blk_cleanup_queue(msb->queue);
1335
+	blk_mq_free_tag_set(&msb->tag_set);
13281336 	msb->queue = NULL;
13291337 
13301338 	sysfs_remove_group(&card->dev.kobj, &msb->attr_group);
....@@ -1344,8 +1352,9 @@
13441352 	struct mspro_block_data *msb = memstick_get_drvdata(card);
13451353 	unsigned long flags;
13461354 
1355
+	blk_mq_stop_hw_queues(msb->queue);
1356
+
13471357 	spin_lock_irqsave(&msb->q_lock, flags);
1348
-	blk_stop_queue(msb->queue);
13491358 	msb->active = 0;
13501359 	spin_unlock_irqrestore(&msb->q_lock, flags);
13511360 
....@@ -1355,7 +1364,6 @@
13551364 static int mspro_block_resume(struct memstick_dev *card)
13561365 {
13571366 	struct mspro_block_data *msb = memstick_get_drvdata(card);
1358
-	unsigned long flags;
13591367 	int rc = 0;
13601368 
13611369 #ifdef CONFIG_MEMSTICK_UNSAFE_RESUME
....@@ -1401,9 +1409,7 @@
14011409 
14021410 #endif /* CONFIG_MEMSTICK_UNSAFE_RESUME */
14031411 
1404
-	spin_lock_irqsave(&msb->q_lock, flags);
1405
-	blk_start_queue(msb->queue);
1406
-	spin_unlock_irqrestore(&msb->q_lock, flags);
1412
+	blk_mq_start_hw_queues(msb->queue);
14071413 	return rc;
14081414 }
14091415