@@ -251,14 +251,69 @@ static int ufshcd_mcq_config_nr_queues(struct ufs_hba *hba)
return 0;
}
+int ufshcd_mcq_memory_alloc(struct ufs_hba *hba)
+{
+ struct ufs_hw_queue *hwq;
+ size_t utrdl_size, cqe_size;
+ int i;
+
+ for (i = 0; i < hba->nr_hw_queues; i++) {
+ hwq = &hba->uhq[i];
+
+ utrdl_size = sizeof(struct utp_transfer_req_desc) *
+ hwq->max_entries;
+ hwq->sqe_base_addr = dmam_alloc_coherent(hba->dev, utrdl_size,
+ &hwq->sqe_dma_addr,
+ GFP_KERNEL);
+ if (!hwq->sqe_dma_addr) {
+ dev_err(hba->dev, "SQE allocation failed\n");
+ return -ENOMEM;
+ }
+
+ cqe_size = sizeof(struct cq_entry) * hwq->max_entries;
+ hwq->cqe_base_addr = dmam_alloc_coherent(hba->dev, cqe_size,
+ &hwq->cqe_dma_addr,
+ GFP_KERNEL);
+ if (!hwq->cqe_dma_addr) {
+ dev_err(hba->dev, "CQE allocation failed\n");
+ return -ENOMEM;
+ }
+ }
+
+ return 0;
+}
+
int ufshcd_mcq_init(struct ufs_hba *hba)
{
- int ret;
+ int ret, i;
+ struct ufs_hw_queue *hwq;
ret = ufshcd_mcq_config_nr_queues(hba);
if (ret)
return ret;
ret = ufshcd_mcq_config_resource(hba);
- return ret;
+ if (ret)
+ return ret;
+
+ hba->uhq = devm_kmalloc(hba->dev,
+ hba->nr_hw_queues * sizeof(struct ufs_hw_queue),
+ GFP_KERNEL);
+ if (!hba->uhq) {
+ dev_err(hba->dev, "Alloc ufs hw queue failed\n");
+ ret = -ENOMEM;
+ return ret;
+ }
+
+ for (i = 0; i < hba->nr_hw_queues; i++) {
+ hwq = &hba->uhq[i];
+ hwq->max_entries = hba->nutrs;
+ }
+
+ /* The very first HW queue is to serve device command */
+ hba->dev_cmd_queue = &hba->uhq[0];
+ /* Give dev_cmd_queue the minimal number of entries */
+ hba->dev_cmd_queue->max_entries = 2;
+
+ return 0;
}
@@ -52,6 +52,7 @@ int ufshcd_query_flag(struct ufs_hba *hba, enum query_opcode opcode,
void ufshcd_auto_hibern8_update(struct ufs_hba *hba, u32 ahit);
int ufshcd_mcq_init(struct ufs_hba *hba);
u32 ufshcd_mcq_decide_queue_depth(struct ufs_hba *hba);
+int ufshcd_mcq_memory_alloc(struct ufs_hba *hba);
#define SD_ASCII_STD true
#define SD_RAW false
@@ -3676,6 +3676,8 @@ static int ufshcd_memory_alloc(struct ufs_hba *hba)
goto out;
}
+ if (hba->utmrdl_base_addr)
+ goto skip_utmrdl;
/*
* Allocate memory for UTP Task Management descriptors
* UFSHCI requires 1024 byte alignment of UTMRD
@@ -3692,6 +3694,7 @@ static int ufshcd_memory_alloc(struct ufs_hba *hba)
goto out;
}
+skip_utmrdl:
/* Allocate memory for local reference block */
hba->lrb = devm_kcalloc(hba->dev,
hba->nutrs, sizeof(struct ufshcd_lrb),
@@ -8173,6 +8176,22 @@ static int ufshcd_add_lus(struct ufs_hba *hba)
return ret;
}
+/* SDB - Single Doorbell */
+static void ufshcd_release_sdb_queue(struct ufs_hba *hba, int nutrs)
+{
+ size_t ucdl_size, utrdl_size;
+
+ ucdl_size = sizeof(struct utp_transfer_cmd_desc) * nutrs;
+ dmam_free_coherent(hba->dev, ucdl_size, hba->ucdl_base_addr,
+ hba->ucdl_dma_addr);
+
+ utrdl_size = sizeof(struct utp_transfer_req_desc) * nutrs;
+ dmam_free_coherent(hba->dev, utrdl_size, hba->utrdl_base_addr,
+ hba->utrdl_dma_addr);
+
+ devm_kfree(hba->dev, hba->lrb);
+}
+
static int ufshcd_config_mcq(struct ufs_hba *hba)
{
int ret;
@@ -8180,11 +8199,25 @@ static int ufshcd_config_mcq(struct ufs_hba *hba)
hba->nutrs = ufshcd_mcq_decide_queue_depth(hba);
ret = ufshcd_mcq_init(hba);
- if (ret) {
- hba->nutrs = old_nutrs;
- return ret;
+ if (ret)
+ goto err;
+
+ if (hba->nutrs != old_nutrs) {
+ ufshcd_release_sdb_queue(hba, old_nutrs);
+ ret = ufshcd_memory_alloc(hba);
+ if (ret)
+ goto err;
+ ufshcd_host_memory_configure(hba);
}
+
+ ret = ufshcd_mcq_memory_alloc(hba);
+ if (ret)
+ goto err;
+
return 0;
+err:
+ hba->nutrs = old_nutrs;
+ return ret;
}
/**
@@ -857,6 +857,8 @@ enum ufshcd_res {
* @nr_queues: number of Queues of different queue types
* @res: array of resource info of MCQ registers
* @mcq_base: Multi circular queue registers base address
+ * @uhq: array of supported hardware queues
+ * @dev_cmd_queue: Queue for issuing device management commands
*/
struct ufs_hba {
void __iomem *mmio_base;
@@ -1011,6 +1013,23 @@ struct ufs_hba {
unsigned int nr_queues[HCTX_MAX_TYPES];
struct ufshcd_res_info_t res[RES_MAX];
void __iomem *mcq_base;
+ struct ufs_hw_queue *uhq;
+ struct ufs_hw_queue *dev_cmd_queue;
+};
+
+/**
+ * @sqe_base_addr: submission queue entry base address
+ * @sqe_dma_addr: submission queue dma address
+ * @cqe_base_addr: completion queue base address
+ * @cqe_dma_addr: completion queue dma address
+ * @max_entries: max number of slots in this hardware queue
+ */
+struct ufs_hw_queue {
+ void *sqe_base_addr;
+ dma_addr_t sqe_dma_addr;
+ struct cq_entry *cqe_base_addr;
+ dma_addr_t cqe_dma_addr;
+ u32 max_entries;
};
static inline bool is_mcq_supported(struct ufs_hba *hba)
@@ -490,6 +490,27 @@ struct utp_transfer_req_desc {
__le16 prd_table_offset;
};
+struct cq_entry {
+ /* DW 0-1 */
+ __le64 command_desc_base_addr;
+
+ /* DW 2 */
+ __le16 response_upiu_length;
+ __le16 response_upiu_offset;
+
+ /* DW 3 */
+ __le16 prd_table_length;
+ __le16 prd_table_offset;
+
+ /* DW 4 */
+ __le32 status;
+
+ /* DW 5-7 */
+ u32 reserved[3];
+};
+
+static_assert(sizeof(struct cq_entry) == 32);
+
/*
* UTMRD structure.
*/