1 files changed, 159 insertions, 51 deletions

diff --git a/sys/dev/ufshci/ufshci_req_sdb.c b/sys/dev/ufshci/ufshci_req_sdb.c
index b1f303afaef5..ca47aa159c5b 100644
--- a/sys/dev/ufshci/ufshci_req_sdb.c
+++ b/sys/dev/ufshci/ufshci_req_sdb.c
@@ -26,12 +26,6 @@ ufshci_req_sdb_cmd_desc_destroy(struct ufshci_req_queue *req_queue)
 		tr = hwq->act_tr[i];
 		bus_dmamap_destroy(req_queue->dma_tag_payload,
 		    tr->payload_dma_map);
-		free(tr, M_UFSHCI);
-	}
-
-	if (hwq->act_tr) {
-		free(hwq->act_tr, M_UFSHCI);
-		hwq->act_tr = NULL;
 	}
 
 	if (req_queue->ucd) {
@@ -46,6 +40,8 @@ ufshci_req_sdb_cmd_desc_destroy(struct ufshci_req_queue *req_queue)
 		bus_dma_tag_destroy(req_queue->dma_tag_ucd);
 		req_queue->dma_tag_ucd = NULL;
 	}
+
+	free(req_queue->hwq->ucd_bus_addr, M_UFSHCI);
 }
 
 static void
@@ -76,11 +72,14 @@ ufshci_req_sdb_cmd_desc_construct(struct ufshci_req_queue *req_queue,
     uint32_t num_entries, struct ufshci_controller *ctrlr)
 {
 	struct ufshci_hw_queue *hwq = &req_queue->hwq[UFSHCI_SDB_Q];
-	struct ufshci_tracker *tr;
 	size_t ucd_allocsz, payload_allocsz;
 	uint8_t *ucdmem;
 	int i, error;
 
+	req_queue->hwq->ucd_bus_addr = malloc(sizeof(bus_addr_t) *
+		req_queue->num_trackers,
+	    M_UFSHCI, M_ZERO | M_NOWAIT);
+
 	/*
 	 * Each component must be page aligned, and individual PRP lists
 	 * cannot cross a page boundary.
@@ -134,27 +133,14 @@ ufshci_req_sdb_cmd_desc_construct(struct ufshci_req_queue *req_queue,
 		goto out;
 	}
 
-	hwq->act_tr = malloc_domainset(sizeof(struct ufshci_tracker *) *
-		req_queue->num_entries,
-	    M_UFSHCI, DOMAINSET_PREF(req_queue->domain), M_ZERO | M_WAITOK);
-
 	for (i = 0; i < req_queue->num_trackers; i++) {
-		tr = malloc_domainset(sizeof(struct ufshci_tracker), M_UFSHCI,
-		    DOMAINSET_PREF(req_queue->domain), M_ZERO | M_WAITOK);
-
 		bus_dmamap_create(req_queue->dma_tag_payload, 0,
-		    &tr->payload_dma_map);
+		    &hwq->act_tr[i]->payload_dma_map);
 
-		tr->req_queue = req_queue;
-		tr->slot_num = i;
-		tr->slot_state = UFSHCI_SLOT_STATE_FREE;
-
-		tr->ucd = (struct ufshci_utp_cmd_desc *)ucdmem;
-		tr->ucd_bus_addr = hwq->ucd_bus_addr[i];
+		hwq->act_tr[i]->ucd = (struct ufshci_utp_cmd_desc *)ucdmem;
+		hwq->act_tr[i]->ucd_bus_addr = hwq->ucd_bus_addr[i];
 
 		ucdmem += sizeof(struct ufshci_utp_cmd_desc);
-
-		hwq->act_tr[i] = tr;
 	}
 
 	return (0);
@@ -163,25 +149,19 @@ out:
 	return (ENOMEM);
 }
 
-static bool
-ufshci_req_sdb_is_doorbell_cleared(struct ufshci_controller *ctrlr,
-    uint8_t slot)
-{
-	uint32_t utrldbr;
-
-	utrldbr = ufshci_mmio_read_4(ctrlr, utrldbr);
-	return (!(utrldbr & (1 << slot)));
-}
-
 int
 ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
     struct ufshci_req_queue *req_queue, uint32_t num_entries, bool is_task_mgmt)
 {
 	struct ufshci_hw_queue *hwq;
-	size_t allocsz;
+	size_t desc_size, alloc_size;
 	uint64_t queuemem_phys;
 	uint8_t *queuemem;
-	int error;
+	struct ufshci_tracker *tr;
+	const size_t lock_name_len = 32;
+	char qlock_name[lock_name_len], recovery_lock_name[lock_name_len];
+	char *base;
+	int i, error;
 
 	req_queue->ctrlr = ctrlr;
 	req_queue->is_task_mgmt = is_task_mgmt;
@@ -198,21 +178,34 @@ ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
 	hwq = &req_queue->hwq[UFSHCI_SDB_Q];
 	hwq->num_entries = req_queue->num_entries;
 	hwq->num_trackers = req_queue->num_trackers;
-	req_queue->hwq->ucd_bus_addr = malloc(sizeof(bus_addr_t) *
-		req_queue->num_trackers,
-	    M_UFSHCI, M_ZERO | M_NOWAIT);
+	hwq->ctrlr = ctrlr;
+	hwq->req_queue = req_queue;
+
+	base = is_task_mgmt ? "ufshci utmrq" : "ufshci utrq";
+	snprintf(qlock_name, sizeof(qlock_name), "%s #%d lock", base,
+	    UFSHCI_SDB_Q);
+	snprintf(recovery_lock_name, sizeof(recovery_lock_name),
+	    "%s #%d recovery lock", base, UFSHCI_SDB_Q);
 
-	mtx_init(&hwq->qlock, "ufshci req_queue lock", NULL, MTX_DEF);
+	mtx_init(&hwq->qlock, qlock_name, NULL, MTX_DEF);
+	mtx_init(&hwq->recovery_lock, recovery_lock_name, NULL, MTX_DEF);
+
+	callout_init_mtx(&hwq->timer, &hwq->recovery_lock, 0);
+	hwq->timer_armed = false;
+	hwq->recovery_state = RECOVERY_WAITING;
 
 	/*
 	 * Allocate physical memory for request queue (UTP Transfer Request
 	 * Descriptor (UTRD) or UTP Task Management Request Descriptor (UTMRD))
 	 * Note: UTRD/UTMRD format is restricted to 1024-byte alignment.
 	 */
-	allocsz = num_entries * sizeof(struct ufshci_utp_xfer_req_desc);
+	desc_size = is_task_mgmt ?
+	    sizeof(struct ufshci_utp_task_mgmt_req_desc) :
+	    sizeof(struct ufshci_utp_xfer_req_desc);
+	alloc_size = num_entries * desc_size;
 	error = bus_dma_tag_create(bus_get_dma_tag(ctrlr->dev), 1024,
 	    ctrlr->page_size, BUS_SPACE_MAXADDR, BUS_SPACE_MAXADDR, NULL, NULL,
-	    allocsz, 1, allocsz, 0, NULL, NULL, &hwq->dma_tag_queue);
+	    alloc_size, 1, alloc_size, 0, NULL, NULL, &hwq->dma_tag_queue);
 	if (error != 0) {
 		ufshci_printf(ctrlr, "request queue tag create failed %d\n",
 		    error);
@@ -227,7 +220,7 @@ ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
 	}
 
 	if (bus_dmamap_load(hwq->dma_tag_queue, hwq->queuemem_map, queuemem,
-		allocsz, ufshci_single_map, &queuemem_phys, 0) != 0) {
+		alloc_size, ufshci_single_map, &queuemem_phys, 0) != 0) {
 		ufshci_printf(ctrlr, "failed to load request queue memory\n");
 		bus_dmamem_free(hwq->dma_tag_queue, hwq->utrd,
 		    hwq->queuemem_map);
@@ -238,13 +231,34 @@ ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
 	hwq->num_intr_handler_calls = 0;
 	hwq->num_retries = 0;
 	hwq->num_failures = 0;
-	hwq->utrd = (struct ufshci_utp_xfer_req_desc *)queuemem;
 	hwq->req_queue_addr = queuemem_phys;
 
+	/* Allocate trackers */
+	hwq->act_tr = malloc_domainset(sizeof(struct ufshci_tracker *) *
+		req_queue->num_entries,
+	    M_UFSHCI, DOMAINSET_PREF(req_queue->domain), M_ZERO | M_WAITOK);
+
+	TAILQ_INIT(&hwq->free_tr);
+	TAILQ_INIT(&hwq->outstanding_tr);
+
+	for (i = 0; i < req_queue->num_trackers; i++) {
+		tr = malloc_domainset(sizeof(struct ufshci_tracker), M_UFSHCI,
+		    DOMAINSET_PREF(req_queue->domain), M_ZERO | M_WAITOK);
+
+		tr->req_queue = req_queue;
+		tr->slot_num = i;
+		tr->slot_state = UFSHCI_SLOT_STATE_FREE;
+		TAILQ_INSERT_HEAD(&hwq->free_tr, tr, tailq);
+
+		hwq->act_tr[i] = tr;
+	}
+
 	if (is_task_mgmt) {
 		/* UTP Task Management Request (UTMR) */
 		uint32_t utmrlba, utmrlbau;
 
+		hwq->utmrd = (struct ufshci_utp_task_mgmt_req_desc *)queuemem;
+
 		utmrlba = hwq->req_queue_addr & 0xffffffff;
 		utmrlbau = hwq->req_queue_addr >> 32;
 		ufshci_mmio_write_4(ctrlr, utmrlba, utmrlba);
@@ -253,6 +267,8 @@ ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
 		/* UTP Transfer Request (UTR) */
 		uint32_t utrlba, utrlbau;
 
+		hwq->utrd = (struct ufshci_utp_xfer_req_desc *)queuemem;
+
 		/*
 		 * Allocate physical memory for the command descriptor.
 		 * UTP Transfer Request (UTR) requires memory for a separate
@@ -262,8 +278,6 @@ ufshci_req_sdb_construct(struct ufshci_controller *ctrlr,
 			ctrlr) != 0) {
 			ufshci_printf(ctrlr,
 			    "failed to construct cmd descriptor memory\n");
-			bus_dmamem_free(hwq->dma_tag_queue, hwq->utrd,
-			    hwq->queuemem_map);
 			goto out;
 		}
 
@@ -284,10 +298,27 @@ ufshci_req_sdb_destroy(struct ufshci_controller *ctrlr,
     struct ufshci_req_queue *req_queue)
 {
 	struct ufshci_hw_queue *hwq = &req_queue->hwq[UFSHCI_SDB_Q];
+	struct ufshci_tracker *tr;
+	int i;
+
+	mtx_lock(&hwq->recovery_lock);
+	hwq->timer_armed = false;
+	mtx_unlock(&hwq->recovery_lock);
+	callout_drain(&hwq->timer);
 
 	if (!req_queue->is_task_mgmt)
 		ufshci_req_sdb_cmd_desc_destroy(&ctrlr->transfer_req_queue);
 
+	for (i = 0; i < req_queue->num_trackers; i++) {
+		tr = hwq->act_tr[i];
+		free(tr, M_UFSHCI);
+	}
+
+	if (hwq->act_tr) {
+		free(hwq->act_tr, M_UFSHCI);
+		hwq->act_tr = NULL;
+	}
+
 	if (hwq->utrd != NULL) {
 		bus_dmamap_unload(hwq->dma_tag_queue, hwq->queuemem_map);
 		bus_dmamem_free(hwq->dma_tag_queue, hwq->utrd,
@@ -300,10 +331,11 @@ ufshci_req_sdb_destroy(struct ufshci_controller *ctrlr,
 		hwq->dma_tag_queue = NULL;
 	}
 
+	if (mtx_initialized(&hwq->recovery_lock))
+		mtx_destroy(&hwq->recovery_lock);
 	if (mtx_initialized(&hwq->qlock))
 		mtx_destroy(&hwq->qlock);
 
-	free(req_queue->hwq->ucd_bus_addr, M_UFSHCI);
 	free(req_queue->hwq, M_UFSHCI);
 }
 
@@ -313,10 +345,36 @@ ufshci_req_sdb_get_hw_queue(struct ufshci_req_queue *req_queue)
 	return &req_queue->hwq[UFSHCI_SDB_Q];
 }
 
+void
+ufshci_req_sdb_disable(struct ufshci_controller *ctrlr,
+    struct ufshci_req_queue *req_queue)
+{
+	struct ufshci_hw_queue *hwq = &req_queue->hwq[UFSHCI_SDB_Q];
+	struct ufshci_tracker *tr, *tr_temp;
+
+	mtx_lock(&hwq->recovery_lock);
+	mtx_lock(&hwq->qlock);
+
+	if (mtx_initialized(&hwq->recovery_lock))
+		mtx_assert(&hwq->recovery_lock, MA_OWNED);
+	if (mtx_initialized(&hwq->qlock))
+		mtx_assert(&hwq->qlock, MA_OWNED);
+
+	hwq->recovery_state = RECOVERY_WAITING;
+	TAILQ_FOREACH_SAFE(tr, &hwq->outstanding_tr, tailq, tr_temp) {
+		tr->deadline = SBT_MAX;
+	}
+
+	mtx_unlock(&hwq->qlock);
+	mtx_unlock(&hwq->recovery_lock);
+}
+
 int
 ufshci_req_sdb_enable(struct ufshci_controller *ctrlr,
     struct ufshci_req_queue *req_queue)
 {
+	struct ufshci_hw_queue *hwq = &req_queue->hwq[UFSHCI_SDB_Q];
+
 	if (req_queue->is_task_mgmt) {
 		uint32_t hcs, utmrldbr, utmrlrsr;
 
@@ -368,6 +426,14 @@ ufshci_req_sdb_enable(struct ufshci_controller *ctrlr,
 		ufshci_mmio_write_4(ctrlr, utrlrsr, utrlrsr);
 	}
 
+	if (mtx_initialized(&hwq->recovery_lock))
+		mtx_assert(&hwq->recovery_lock, MA_OWNED);
+	if (mtx_initialized(&hwq->qlock))
+		mtx_assert(&hwq->qlock, MA_OWNED);
+	KASSERT(!req_queue->ctrlr->is_failed, ("Enabling a failed hwq\n"));
+
+	hwq->recovery_state = RECOVERY_NONE;
+
 	return (0);
 }
 
@@ -389,7 +455,18 @@ ufshci_req_sdb_reserve_slot(struct ufshci_req_queue *req_queue,
 }
 
 void
-ufshci_req_sdb_clear_cpl_ntf(struct ufshci_controller *ctrlr,
+ufshci_req_sdb_utmr_clear_cpl_ntf(struct ufshci_controller *ctrlr,
+    struct ufshci_tracker *tr)
+{
+	/*
+	 * NOP
+	 * UTP Task Management does not have a Completion Notification
+	 * Register.
+	 */
+}
+
+void
+ufshci_req_sdb_utr_clear_cpl_ntf(struct ufshci_controller *ctrlr,
     struct ufshci_tracker *tr)
 {
 	uint32_t utrlcnr;
@@ -399,7 +476,19 @@ ufshci_req_sdb_clear_cpl_ntf(struct ufshci_controller *ctrlr,
 }
 
 void
-ufshci_req_sdb_ring_doorbell(struct ufshci_controller *ctrlr,
+ufshci_req_sdb_utmr_ring_doorbell(struct ufshci_controller *ctrlr,
+    struct ufshci_tracker *tr)
+{
+	uint32_t utmrldbr = 0;
+
+	utmrldbr |= 1 << tr->slot_num;
+	ufshci_mmio_write_4(ctrlr, utmrldbr, utmrldbr);
+
+	tr->req_queue->hwq[UFSHCI_SDB_Q].num_cmds++;
+}
+
+void
+ufshci_req_sdb_utr_ring_doorbell(struct ufshci_controller *ctrlr,
     struct ufshci_tracker *tr)
 {
 	uint32_t utrldbr = 0;
@@ -408,9 +497,26 @@ ufshci_req_sdb_ring_doorbell(struct ufshci_controller *ctrlr,
 	ufshci_mmio_write_4(ctrlr, utrldbr, utrldbr);
 
 	tr->req_queue->hwq[UFSHCI_SDB_Q].num_cmds++;
+}
+
+bool
+ufshci_req_sdb_utmr_is_doorbell_cleared(struct ufshci_controller *ctrlr,
+    uint8_t slot)
+{
+	uint32_t utmrldbr;
 
-	// utrldbr = ufshci_mmio_read_4(ctrlr, utrldbr);
-	// printf("DB=0x%08x\n", utrldbr);
+	utmrldbr = ufshci_mmio_read_4(ctrlr, utmrldbr);
+	return (!(utmrldbr & (1 << slot)));
+}
+
+bool
+ufshci_req_sdb_utr_is_doorbell_cleared(struct ufshci_controller *ctrlr,
+    uint8_t slot)
+{
+	uint32_t utrldbr;
+
+	utrldbr = ufshci_mmio_read_4(ctrlr, utrldbr);
+	return (!(utrldbr & (1 << slot)));
 }
 
 bool
@@ -421,6 +527,8 @@ ufshci_req_sdb_process_cpl(struct ufshci_req_queue *req_queue)
 	uint8_t slot;
 	bool done = false;
 
+	mtx_assert(&hwq->recovery_lock, MA_OWNED);
+
 	hwq->num_intr_handler_calls++;
 
 	bus_dmamap_sync(hwq->dma_tag_queue, hwq->queuemem_map,
@@ -435,7 +543,7 @@ ufshci_req_sdb_process_cpl(struct ufshci_req_queue *req_queue)
 		 * is cleared.
 		 */
 		if (tr->slot_state == UFSHCI_SLOT_STATE_SCHEDULED &&
-		    ufshci_req_sdb_is_doorbell_cleared(req_queue->ctrlr,
+		    req_queue->qops.is_doorbell_cleared(req_queue->ctrlr,
 			slot)) {
 			ufshci_req_queue_complete_tracker(tr);
 			done = true;