forked from ~ljy/RK356X_SDK_RELEASE
blame | history | patch | commit | commitdiff
hc
2024-05-10 61598093bbdd283a7edc367d900f223070ead8d2 
 kernel/drivers/block/xsysace.c
....@@ -1,11 +1,8 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  * Xilinx SystemACE device driver
34  *
45  * Copyright 2007 Secret Lab Technologies Ltd.
5
- *
6
- * This program is free software; you can redistribute it and/or modify it
7
- * under the terms of the GNU General Public License version 2 as published
8
- * by the Free Software Foundation.
96  */
107 
118 /*
....@@ -88,7 +85,7 @@
8885 #include <linux/kernel.h>
8986 #include <linux/delay.h>
9087 #include <linux/slab.h>
91
-#include <linux/blkdev.h>
88
+#include <linux/blk-mq.h>
9289 #include <linux/mutex.h>
9390 #include <linux/ata.h>
9491 #include <linux/hdreg.h>
....@@ -209,6 +206,8 @@
209206 	struct device *dev;
210207 	struct request_queue *queue;
211208 	struct gendisk *gd;
209
+	struct blk_mq_tag_set tag_set;
210
+	struct list_head rq_list;
212211 
213212 	/* Inserted CF card parameters */
214213 	u16 cf_id[ATA_ID_WORDS];
....@@ -444,11 +443,16 @@
444443 #define ACE_FSM_NUM_STATES              11
445444 
446445 /* Set flag to exit FSM loop and reschedule tasklet */
447
-static inline void ace_fsm_yield(struct ace_device *ace)
446
+static inline void ace_fsm_yieldpoll(struct ace_device *ace)
448447 {
449
-	dev_dbg(ace->dev, "ace_fsm_yield()\n");
450448 	tasklet_schedule(&ace->fsm_tasklet);
451449 	ace->fsm_continue_flag = 0;
450
+}
451
+
452
+static inline void ace_fsm_yield(struct ace_device *ace)
453
+{
454
+	dev_dbg(ace->dev, "%s()\n", __func__);
455
+	ace_fsm_yieldpoll(ace);
452456 }
453457 
454458 /* Set flag to exit FSM loop and wait for IRQ to reschedule tasklet */
....@@ -456,24 +460,32 @@
456460 {
457461 	dev_dbg(ace->dev, "ace_fsm_yieldirq()\n");
458462 
459
-	if (!ace->irq)
460
-		/* No IRQ assigned, so need to poll */
461
-		tasklet_schedule(&ace->fsm_tasklet);
462
-	ace->fsm_continue_flag = 0;
463
+	if (ace->irq > 0)
464
+		ace->fsm_continue_flag = 0;
465
+	else
466
+		ace_fsm_yieldpoll(ace);
467
+}
468
+
469
+static bool ace_has_next_request(struct request_queue *q)
470
+{
471
+	struct ace_device *ace = q->queuedata;
472
+
473
+	return !list_empty(&ace->rq_list);
463474 }
464475 
465476 /* Get the next read/write request; ending requests that we don't handle */
466477 static struct request *ace_get_next_request(struct request_queue *q)
467478 {
468
-	struct request *req;
479
+	struct ace_device *ace = q->queuedata;
480
+	struct request *rq;
469481 
470
-	while ((req = blk_peek_request(q)) != NULL) {
471
-		if (!blk_rq_is_passthrough(req))
472
-			break;
473
-		blk_start_request(req);
474
-		__blk_end_request_all(req, BLK_STS_IOERR);
482
+	rq = list_first_entry_or_null(&ace->rq_list, struct request, queuelist);
483
+	if (rq) {
484
+		list_del_init(&rq->queuelist);
485
+		blk_mq_start_request(rq);
475486 	}
476
-	return req;
487
+
488
+	return NULL;
477489 }
478490 
479491 static void ace_fsm_dostate(struct ace_device *ace)
....@@ -499,11 +511,11 @@
499511 
500512 		/* Drop all in-flight and pending requests */
501513 		if (ace->req) {
502
-			__blk_end_request_all(ace->req, BLK_STS_IOERR);
514
+			blk_mq_end_request(ace->req, BLK_STS_IOERR);
503515 			ace->req = NULL;
504516 		}
505
-		while ((req = blk_fetch_request(ace->queue)) != NULL)
506
-			__blk_end_request_all(req, BLK_STS_IOERR);
517
+		while ((req = ace_get_next_request(ace->queue)) != NULL)
518
+			blk_mq_end_request(req, BLK_STS_IOERR);
507519 
508520 		/* Drop back to IDLE state and notify waiters */
509521 		ace->fsm_state = ACE_FSM_STATE_IDLE;
....@@ -517,7 +529,7 @@
517529 	switch (ace->fsm_state) {
518530 	case ACE_FSM_STATE_IDLE:
519531 		/* See if there is anything to do */
520
-		if (ace->id_req_count || ace_get_next_request(ace->queue)) {
532
+		if (ace->id_req_count || ace_has_next_request(ace->queue)) {
521533 			ace->fsm_iter_num++;
522534 			ace->fsm_state = ACE_FSM_STATE_REQ_LOCK;
523535 			mod_timer(&ace->stall_timer, jiffies + HZ);
....@@ -651,7 +663,6 @@
651663 			ace->fsm_state = ACE_FSM_STATE_IDLE;
652664 			break;
653665 		}
654
-		blk_start_request(req);
655666 
656667 		/* Okay, it's a data request, set it up for transfer */
657668 		dev_dbg(ace->dev,
....@@ -728,7 +739,8 @@
728739 		}
729740 
730741 		/* bio finished; is there another one? */
731
-		if (__blk_end_request_cur(ace->req, BLK_STS_OK)) {
742
+		if (blk_update_request(ace->req, BLK_STS_OK,
743
+		    blk_rq_cur_bytes(ace->req))) {
732744 			/* dev_dbg(ace->dev, "next block; h=%u c=%u\n",
733745 			 *      blk_rq_sectors(ace->req),
734746 			 *      blk_rq_cur_sectors(ace->req));
....@@ -854,17 +866,23 @@
854866 /* ---------------------------------------------------------------------
855867  * Block ops
856868  */
857
-static void ace_request(struct request_queue * q)
869
+static blk_status_t ace_queue_rq(struct blk_mq_hw_ctx *hctx,
870
+				 const struct blk_mq_queue_data *bd)
858871 {
859
-	struct request *req;
860
-	struct ace_device *ace;
872
+	struct ace_device *ace = hctx->queue->queuedata;
873
+	struct request *req = bd->rq;
861874 
862
-	req = ace_get_next_request(q);
863
-
864
-	if (req) {
865
-		ace = req->rq_disk->private_data;
866
-		tasklet_schedule(&ace->fsm_tasklet);
875
+	if (blk_rq_is_passthrough(req)) {
876
+		blk_mq_start_request(req);
877
+		return BLK_STS_IOERR;
867878 	}
879
+
880
+	spin_lock_irq(&ace->lock);
881
+	list_add_tail(&req->queuelist, &ace->rq_list);
882
+	spin_unlock_irq(&ace->lock);
883
+
884
+	tasklet_schedule(&ace->fsm_tasklet);
885
+	return BLK_STS_OK;
868886 }
869887 
870888 static unsigned int ace_check_events(struct gendisk *gd, unsigned int clearing)
....@@ -875,26 +893,20 @@
875893 	return ace->media_change ? DISK_EVENT_MEDIA_CHANGE : 0;
876894 }
877895 
878
-static int ace_revalidate_disk(struct gendisk *gd)
896
+static void ace_media_changed(struct ace_device *ace)
879897 {
880
-	struct ace_device *ace = gd->private_data;
881898 	unsigned long flags;
882899 
883
-	dev_dbg(ace->dev, "ace_revalidate_disk()\n");
900
+	dev_dbg(ace->dev, "requesting cf id and scheduling tasklet\n");
884901 
885
-	if (ace->media_change) {
886
-		dev_dbg(ace->dev, "requesting cf id and scheduling tasklet\n");
902
+	spin_lock_irqsave(&ace->lock, flags);
903
+	ace->id_req_count++;
904
+	spin_unlock_irqrestore(&ace->lock, flags);
887905 
888
-		spin_lock_irqsave(&ace->lock, flags);
889
-		ace->id_req_count++;
890
-		spin_unlock_irqrestore(&ace->lock, flags);
891
-
892
-		tasklet_schedule(&ace->fsm_tasklet);
893
-		wait_for_completion(&ace->id_completion);
894
-	}
906
+	tasklet_schedule(&ace->fsm_tasklet);
907
+	wait_for_completion(&ace->id_completion);
895908 
896909 	dev_dbg(ace->dev, "revalidate complete\n");
897
-	return ace->id_result;
898910 }
899911 
900912 static int ace_open(struct block_device *bdev, fmode_t mode)
....@@ -909,7 +921,8 @@
909921 	ace->users++;
910922 	spin_unlock_irqrestore(&ace->lock, flags);
911923 
912
-	check_disk_change(bdev);
924
+	if (bdev_check_media_change(bdev) && ace->media_change)
925
+		ace_media_changed(ace);
913926 	mutex_unlock(&xsysace_mutex);
914927 
915928 	return 0;
....@@ -953,8 +966,11 @@
953966 	.open = ace_open,
954967 	.release = ace_release,
955968 	.check_events = ace_check_events,
956
-	.revalidate_disk = ace_revalidate_disk,
957969 	.getgeo = ace_getgeo,
970
+};
971
+
972
+static const struct blk_mq_ops ace_mq_ops = {
973
+	.queue_rq	= ace_queue_rq,
958974 };
959975 
960976 /* --------------------------------------------------------------------
....@@ -972,6 +988,7 @@
972988 
973989 	spin_lock_init(&ace->lock);
974990 	init_completion(&ace->id_completion);
991
+	INIT_LIST_HEAD(&ace->rq_list);
975992 
976993 	/*
977994 	 * Map the device
....@@ -989,9 +1006,15 @@
9891006 	/*
9901007 	 * Initialize the request queue
9911008 	 */
992
-	ace->queue = blk_init_queue(ace_request, &ace->lock);
993
-	if (ace->queue == NULL)
1009
+	ace->queue = blk_mq_init_sq_queue(&ace->tag_set, &ace_mq_ops, 2,
1010
+						BLK_MQ_F_SHOULD_MERGE);
1011
+	if (IS_ERR(ace->queue)) {
1012
+		rc = PTR_ERR(ace->queue);
1013
+		ace->queue = NULL;
9941014 		goto err_blk_initq;
1015
+	}
1016
+	ace->queue->queuedata = ace;
1017
+
9951018 	blk_queue_logical_block_size(ace->queue, 512);
9961019 	blk_queue_bounce_limit(ace->queue, BLK_BOUNCE_HIGH);
9971020 
....@@ -1005,6 +1028,7 @@
10051028 	ace->gd->major = ace_major;
10061029 	ace->gd->first_minor = ace->id * ACE_NUM_MINORS;
10071030 	ace->gd->fops = &ace_fops;
1031
+	ace->gd->events = DISK_EVENT_MEDIA_CHANGE;
10081032 	ace->gd->queue = ace->queue;
10091033 	ace->gd->private_data = ace;
10101034 	snprintf(ace->gd->disk_name, 32, "xs%c", ace->id + 'a');
....@@ -1034,12 +1058,12 @@
10341058 		ACE_CTRL_DATABUFRDYIRQ | ACE_CTRL_ERRORIRQ);
10351059 
10361060 	/* Now we can hook up the irq handler */
1037
-	if (ace->irq) {
1061
+	if (ace->irq > 0) {
10381062 		rc = request_irq(ace->irq, ace_interrupt, 0, "systemace", ace);
10391063 		if (rc) {
10401064 			/* Failure - fall back to polled mode */
10411065 			dev_err(ace->dev, "request_irq failed\n");
1042
-			ace->irq = 0;
1066
+			ace->irq = rc;
10431067 		}
10441068 	}
10451069 
....@@ -1055,7 +1079,7 @@
10551079 		(unsigned long long) ace->physaddr, ace->baseaddr, ace->irq);
10561080 
10571081 	ace->media_change = 1;
1058
-	ace_revalidate_disk(ace->gd);
1082
+	ace_media_changed(ace);
10591083 
10601084 	/* Make the sysace device 'live' */
10611085 	add_disk(ace->gd);
....@@ -1068,6 +1092,7 @@
10681092 	put_disk(ace->gd);
10691093 err_alloc_disk:
10701094 	blk_cleanup_queue(ace->queue);
1095
+	blk_mq_free_tag_set(&ace->tag_set);
10711096 err_blk_initq:
10721097 	iounmap(ace->baseaddr);
10731098 err_ioremap:
....@@ -1083,12 +1108,14 @@
10831108 		put_disk(ace->gd);
10841109 	}
10851110 
1086
-	if (ace->queue)
1111
+	if (ace->queue) {
10871112 		blk_cleanup_queue(ace->queue);
1113
+		blk_mq_free_tag_set(&ace->tag_set);
1114
+	}
10881115 
10891116 	tasklet_kill(&ace->fsm_tasklet);
10901117 
1091
-	if (ace->irq)
1118
+	if (ace->irq > 0)
10921119 		free_irq(ace->irq, ace);
10931120 
10941121 	iounmap(ace->baseaddr);
....@@ -1100,11 +1127,6 @@
11001127 	struct ace_device *ace;
11011128 	int rc;
11021129 	dev_dbg(dev, "ace_alloc(%p)\n", dev);
1103
-
1104
-	if (!physaddr) {
1105
-		rc = -ENODEV;
1106
-		goto err_noreg;
1107
-	}
11081130 
11091131 	/* Allocate and initialize the ace device structure */
11101132 	ace = kzalloc(sizeof(struct ace_device), GFP_KERNEL);
....@@ -1131,7 +1153,6 @@
11311153 	dev_set_drvdata(dev, NULL);
11321154 	kfree(ace);
11331155 err_alloc:
1134
-err_noreg:
11351156 	dev_err(dev, "could not initialize device, err=%i\n", rc);
11361157 	return rc;
11371158 }
....@@ -1154,10 +1175,11 @@
11541175 
11551176 static int ace_probe(struct platform_device *dev)
11561177 {
1157
-	resource_size_t physaddr = 0;
11581178 	int bus_width = ACE_BUS_WIDTH_16; /* FIXME: should not be hard coded */
1179
+	resource_size_t physaddr;
1180
+	struct resource *res;
11591181 	u32 id = dev->id;
1160
-	int irq = 0;
1182
+	int irq;
11611183 	int i;
11621184 
11631185 	dev_dbg(&dev->dev, "ace_probe(%p)\n", dev);
....@@ -1168,12 +1190,15 @@
11681190 	if (of_find_property(dev->dev.of_node, "8-bit", NULL))
11691191 		bus_width = ACE_BUS_WIDTH_8;
11701192 
1171
-	for (i = 0; i < dev->num_resources; i++) {
1172
-		if (dev->resource[i].flags & IORESOURCE_MEM)
1173
-			physaddr = dev->resource[i].start;
1174
-		if (dev->resource[i].flags & IORESOURCE_IRQ)
1175
-			irq = dev->resource[i].start;
1176
-	}
1193
+	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
1194
+	if (!res)
1195
+		return -EINVAL;
1196
+
1197
+	physaddr = res->start;
1198
+	if (!physaddr)
1199
+		return -ENODEV;
1200
+
1201
+	irq = platform_get_irq_optional(dev, 0);
11771202 
11781203 	/* Call the bus-independent setup code */
11791204 	return ace_alloc(&dev->dev, id, physaddr, irq, bus_width);