// SPDX-License-Identifier: GPL-2.0 /* * Copyright (C) 2022 MediaTek Inc. * Authors: * Eddie Huang */ #include #include #include #include #include #include "ufshcd.h" #include "ufshcd-crypto.h" #include "ufs-mediatek.h" #include "ufs-mediatek-sip.h" #include "ufs-mediatek-trace.h" #include enum { /* one for empty, one for devman command*/ UFSHCD_MCQ_NUM_RESERVED = 2, }; enum { UFSHCD_UIC_DL_PA_INIT_ERROR = (1 << 0), }; #define MTK_MCQ_INVALID_IRQ 0xFFFF #define ufs_mtk_mcq_hex_dump(prefix_str, buf, len) do { \ size_t __len = (len); \ print_hex_dump(KERN_ERR, prefix_str, \ __len > 4 ? DUMP_PREFIX_OFFSET : DUMP_PREFIX_NONE,\ 16, 4, buf, __len, false); \ } while (0) static unsigned int mtk_mcq_irq[UFSHCD_MAX_Q_NR]; static void ufs_mtk_mcq_map_tag(void *data, struct ufs_hba *hba, int index, int *tag); static void ufs_mtk_mcq_print_trs_tag(struct ufs_hba *hba, int tag, bool pr_prdt) { struct ufshcd_lrb *lrbp; int prdt_length; lrbp = &hba->lrb[tag]; dev_err(hba->dev, "UPIU[%d] - issue time %lld us\n", tag, ktime_to_us(lrbp->issue_time_stamp)); dev_err(hba->dev, "UPIU[%d] - complete time %lld us\n", tag, ktime_to_us(lrbp->compl_time_stamp)); dev_err(hba->dev, "UPIU[%d] - Transfer Request Descriptor phys@0x%llx\n", tag, (u64)lrbp->utrd_dma_addr); ufs_mtk_mcq_hex_dump("UPIU TRD: ", lrbp->utr_descriptor_ptr, sizeof(struct utp_transfer_req_desc)); dev_err(hba->dev, "UPIU[%d] - Request UPIU phys@0x%llx\n", tag, (u64)lrbp->ucd_req_dma_addr); ufs_mtk_mcq_hex_dump("UPIU REQ: ", lrbp->ucd_req_ptr, sizeof(struct utp_upiu_req)); dev_err(hba->dev, "UPIU[%d] - Response UPIU phys@0x%llx\n", tag, (u64)lrbp->ucd_rsp_dma_addr); ufs_mtk_mcq_hex_dump("UPIU RSP: ", lrbp->ucd_rsp_ptr, sizeof(struct utp_upiu_rsp)); prdt_length = le16_to_cpu( lrbp->utr_descriptor_ptr->prd_table_length); if (hba->quirks & UFSHCD_QUIRK_PRDT_BYTE_GRAN) prdt_length /= hba->sg_entry_size; dev_err(hba->dev, "UPIU[%d] - PRDT - %d entries phys@0x%llx\n", tag, prdt_length, (u64)lrbp->ucd_prdt_dma_addr); if (pr_prdt) ufs_mtk_mcq_hex_dump("UPIU PRDT: ", lrbp->ucd_prdt_ptr, hba->sg_entry_size * prdt_length); } static void ufs_mtk_mcq_print_trs(void *data, struct ufs_hba *hba, bool pr_prdt) { int tag; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; unsigned long *bitmap = hba_priv->outstanding_mcq_reqs; for_each_set_bit(tag, bitmap, UFSHCD_MAX_TAG) { ufs_mtk_mcq_print_trs_tag(hba, tag, pr_prdt); } } static void ufs_mtk_mcq_add_command_trace(struct ufs_hba *hba, unsigned int tag, enum ufs_trace_str_t str_t) { u64 lba = 0; u8 opcode = 0, group_id = 0; u32 intr = 0xFF, doorbell = 0xFF; struct ufshcd_lrb *lrbp = &hba->lrb[tag]; struct scsi_cmnd *cmd = lrbp->cmd; struct request *rq = scsi_cmd_to_rq(cmd); int transfer_len = -1; if (!cmd) return; /* TODO: port UPIU trace if necessary */ // ufshcd_add_cmd_upiu_trace(hba, tag, str_t); if (!trace_ufs_mtk_mcq_command_enabled()) return; opcode = cmd->cmnd[0]; if (opcode == READ_10 || opcode == WRITE_10) { /* * Currently we only fully trace read(10) and write(10) commands */ transfer_len = be32_to_cpu(lrbp->ucd_req_ptr->sc.exp_data_transfer_len); lba = scsi_get_lba(cmd); if (opcode == WRITE_10) group_id = lrbp->cmd->cmnd[6]; } else if (opcode == UNMAP) { /* * The number of Bytes to be unmapped beginning with the lba. */ transfer_len = blk_rq_bytes(rq); lba = scsi_get_lba(cmd); } trace_ufs_mtk_mcq_command(dev_name(hba->dev), str_t, tag, doorbell, transfer_len, intr, lba, opcode, group_id); } static u32 ufs_mtk_q_entry_offset(struct ufs_queue *q, union utp_q_entry *ptr) { return (ptr - q->q_base_addr); } static dma_addr_t ufs_mtk_q_virt_to_dma(struct ufs_queue *q, union utp_q_entry *entry) { unsigned long q_offset; if (!q || !entry || entry < q->q_base_addr) { //TBD: error message return 0; } q_offset = entry - q->q_base_addr; if (q_offset > q->q_depth) { //TBD: error message return 0; } return (q_offset * SQE_SIZE); } static bool ufs_mtk_is_sq_full(struct ufs_hba *hba, struct ufs_queue *q) { u32 head_offset; u32 tail_offset; head_offset = ufshcd_readl(hba, MCQ_ADDR(REG_UFS_SQ_HEAD, q->qid)) / SQE_SIZE; tail_offset = ufs_mtk_q_entry_offset(q, q->tail); if (((tail_offset + 1) % q->q_depth) == head_offset) return true; else return false; } static inline void ufs_mtk_write_sq_tail(struct ufs_hba *hba, struct ufs_queue *sq_ptr) { ufshcd_writel(hba, ufs_mtk_q_virt_to_dma(sq_ptr, sq_ptr->tail), MCQ_ADDR(REG_UFS_SQ_TAIL, sq_ptr->qid)); sq_ptr->tail_written = sq_ptr->tail; } static bool ufs_mtk_is_cq_empty(struct ufs_hba *hba, struct ufs_queue *q) { unsigned long tail_offset; unsigned long head_offset; tail_offset = ufshcd_readl(hba, MCQ_ADDR(REG_UFS_CQ_TAIL, q->qid)); head_offset = ufs_mtk_q_entry_offset(q, q->head) * CQE_SIZE; if (head_offset == tail_offset) return true; else return false; } static void ufs_mtk_inc_cq_head(struct ufs_hba *hba, struct ufs_queue *q) { u8 head_offset; u8 head_next_offset; u32 head_next_offset_addr; head_offset = ufs_mtk_q_entry_offset(q, q->head); head_next_offset = (head_offset+1) % q->q_depth; q->head = &q->q_base_addr[head_next_offset]; head_next_offset_addr = head_next_offset * CQE_SIZE; ufshcd_writel(hba, head_next_offset_addr, MCQ_ADDR(REG_UFS_CQ_HEAD, q->qid)); } static void ufs_mtk_transfer_req_compl_handler(struct ufs_hba *hba, bool retry_requests, int index) { struct ufshcd_lrb *lrbp; struct scsi_cmnd *cmd; int result; bool update_scaling = false; lrbp = &hba->lrb[index]; lrbp->compl_time_stamp = ktime_get(); cmd = lrbp->cmd; if (cmd) { trace_android_vh_ufs_compl_command(hba, lrbp); ufs_mtk_mcq_add_command_trace(hba, index, UFS_CMD_COMP); result = retry_requests ? DID_BUS_BUSY << 16 : ufshcd_transfer_rsp_status(hba, lrbp); scsi_dma_unmap(cmd); cmd->result = result; ufshcd_crypto_clear_prdt(hba, lrbp); /* Mark completed command as NULL in LRB */ lrbp->cmd = NULL; /* Do not touch lrbp after scsi done */ cmd->scsi_done(cmd); ufshcd_release(hba); update_scaling = true; } else if (lrbp->command_type == UTP_CMD_TYPE_DEV_MANAGE || lrbp->command_type == UTP_CMD_TYPE_UFS_STORAGE) { if (hba->dev_cmd.complete) { trace_android_vh_ufs_compl_command(hba, lrbp); ufs_mtk_mcq_add_command_trace(hba, index, UFS_DEV_COMP); complete(hba->dev_cmd.complete); update_scaling = true; } } if (update_scaling) ufshcd_clk_scaling_update_busy(hba); } static irqreturn_t ufs_mtk_mcq_cq_ring_handler(struct ufs_hba *hba, int hwq) { struct ufs_queue *q; struct utp_cq_entry *entry; struct ufshcd_lrb *lrb; dma_addr_t ucdl_dma_addr; u8 tag_offset; u32 mcq_cqe_ocs; unsigned long flags; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; q = &hba_priv->mcq_q_cfg.cq[hwq]; while (!ufs_mtk_is_cq_empty(hba, q)) { entry = &q->head->cq; ucdl_dma_addr = entry->UCD_base & UCD_BASE_ADD_MASK; tag_offset = (ucdl_dma_addr - hba->ucdl_dma_addr) / sizeof_utp_transfer_cmd_desc(hba); /* Write OCS value back to UTRD, as legacy controller did */ lrb = &hba->lrb[tag_offset]; mcq_cqe_ocs = (le32_to_cpu(entry->dword_4) & UTRD_OCS_MASK); lrb->utr_descriptor_ptr->header.dword_2 = cpu_to_le32(mcq_cqe_ocs); ufs_mtk_inc_cq_head(hba, q); spin_lock_irqsave(&hba->outstanding_lock, flags); clear_bit(tag_offset, hba_priv->outstanding_mcq_reqs); spin_unlock_irqrestore(&hba->outstanding_lock, flags); ufs_mtk_transfer_req_compl_handler(hba, false, tag_offset); } return IRQ_HANDLED; } static irqreturn_t ufs_mtk_mcq_cq_handler(struct ufs_hba *hba) { int i; unsigned long cq_is, flags; irqreturn_t ret = IRQ_HANDLED; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; spin_lock_irqsave(&hba->outstanding_lock, flags); cq_is = ufshcd_readl(hba, REG_UFS_MMIO_CQ_IS); ufshcd_writel(hba, cq_is, REG_UFS_MMIO_CQ_IS); spin_unlock_irqrestore(&hba->outstanding_lock, flags); for (i = 0; i < hba_priv->mcq_nr_hw_queue; i++) { if (test_bit(i, &cq_is)) ret |= ufs_mtk_mcq_cq_ring_handler(hba, i); } return ret; } static irqreturn_t ufs_mtk_mcq_sq_ring_handler(struct ufs_hba *hba, int hwq) { // Do not handle SQ now, TBD return IRQ_HANDLED; } static irqreturn_t ufs_mtk_mcq_sq_handler(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; unsigned long sq_is, flags; int i; irqreturn_t ret = IRQ_NONE; spin_lock_irqsave(&hba->outstanding_lock, flags); sq_is = ufshcd_readl(hba, REG_UFS_MMIO_SQ_IS); ufshcd_writel(hba, sq_is, REG_UFS_MMIO_SQ_IS); spin_unlock_irqrestore(&hba->outstanding_lock, flags); for (i = 0; i < hba_priv->mcq_nr_hw_queue; i++) { if (test_bit(i, &sq_is)) ret |= ufs_mtk_mcq_sq_ring_handler(hba, i); } return ret; } static irqreturn_t ufs_mtk_mcq_intr(int irq, void *__intr_info) { struct ufs_mcq_intr_info *mcq_intr_info = __intr_info; struct ufs_hba *hba = mcq_intr_info->hba; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int hwq; irqreturn_t retval = IRQ_NONE; if (hba_priv->mcq_nr_intr == 0) return IRQ_NONE; hwq = mcq_intr_info->qid; ufshcd_writel(hba, (1 << hwq), REG_UFS_MMIO_CQ_IS); retval = ufs_mtk_mcq_cq_ring_handler(hba, hwq); return retval; } static void ufs_mtk_legacy_mcq_handler(void *data, struct ufs_hba *hba, u32 intr_status, irqreturn_t *retval) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; if (!hba_priv->is_mcq_enabled) { return; } /* If enable multiple CQ interrupt, legacy interrupt should not handle * CQ/SQ interrupt * If enbale CQ interrupt, this function should not be called */ if (hba_priv->mcq_nr_intr > 0) { *retval |= IRQ_HANDLED; return; } if (intr_status & CQ_INT) ufs_mtk_mcq_cq_handler(hba); if (intr_status & SQ_INT) ufs_mtk_mcq_sq_handler(hba); *retval |= IRQ_HANDLED; } static void ufs_mtk_mcq_send_hw_cmd(struct ufs_hba *hba, unsigned int task_tag) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct ufshcd_lrb *lrbp = &hba->lrb[task_tag]; int q_index = lrbp->android_vendor_data1; struct ufs_queue *sq_ptr = &hba_priv->mcq_q_cfg.sq[q_index]; unsigned long flags; hba_priv->mcq_q_cfg.sent_cmd_count[q_index]++; lrbp->issue_time_stamp = ktime_get(); lrbp->compl_time_stamp = ktime_set(0, 0); trace_android_vh_ufs_send_command(hba, lrbp); ufs_mtk_mcq_add_command_trace(hba, task_tag, UFS_CMD_SEND); ufshcd_clk_scaling_start_busy(hba); spin_lock_irqsave(&sq_ptr->q_lock, flags); if (hba->vops && hba->vops->setup_xfer_req) hba->vops->setup_xfer_req(hba, task_tag, (lrbp->cmd ? true : false)); //SQ full happens when CQ speed <<< SQ speed, if (ufs_mtk_is_sq_full(hba, sq_ptr)) { dev_err(hba->dev, "qid: %d FULL", sq_ptr->qid); goto finalize; } set_bit(task_tag, hba_priv->outstanding_mcq_reqs); /* Copy the modified UTRD to the corresponding SQ entry */ memcpy(sq_ptr->tail, lrbp->utr_descriptor_ptr, SQE_SIZE); if (ufs_mtk_q_entry_offset(sq_ptr, ++sq_ptr->tail) == sq_ptr->q_depth) sq_ptr->tail = sq_ptr->q_base_addr; /* Make sure SQE copy finish */ wmb(); ufs_mtk_write_sq_tail(hba, sq_ptr); /* Make sure sq tail update */ wmb(); finalize: spin_unlock_irqrestore(&sq_ptr->q_lock, flags); } static void ufs_mtk_mcq_send_command(void *data, struct ufs_hba *hba, unsigned int task_tag) { ufs_mtk_mcq_send_hw_cmd(hba, task_tag); } static void ufs_mtk_mcq_config_queue(struct ufs_hba *hba, u8 qid, u8 enable, u8 q_type, u16 q_depth, u8 priority, u8 mapping) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct ufs_queue_config *mcq_q_cfg = &hba_priv->mcq_q_cfg; struct ufs_queue *queue; if (q_type == MCQ_Q_TYPE_SQ) { queue = &mcq_q_cfg->sq[qid]; mcq_q_cfg->sq_cq_map[qid] = mapping; } else { queue = &mcq_q_cfg->cq[qid]; } //set depth and priority to 0, if Q is not enabled queue->qid = qid; queue->q_enable = enable; queue->q_type = q_type; queue->q_depth = (enable ? q_depth : 0); queue->priority = (enable ? priority : 0); dev_info(hba->dev, "q_type: %d, qid: %d, enable: %d, q_depth: %d, priority: %d, mapping: %d\n", queue->q_type, queue->qid, queue->q_enable, queue->q_depth, queue->priority, mcq_q_cfg->sq_cq_map[qid]); } static int ufs_mtk_mcq_init_queue(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct ufs_queue_config *mcq_q_cfg = &hba_priv->mcq_q_cfg; int i; u8 sq_nr, cq_nr, q_depth, q_priority = 0; u32 cnt; struct ufs_queue *queue; // queue depth should be the same for every SQ, blk_mq_init_queue q_depth = hba_priv->mcq_nr_q_depth; sq_nr = cq_nr = hba_priv->mcq_nr_hw_queue; for (i = 0; i < sq_nr; i++) { ufs_mtk_mcq_config_queue(hba, i, true, MCQ_Q_TYPE_SQ, q_depth, q_priority, i); ufs_mtk_mcq_config_queue(hba, i, true, MCQ_Q_TYPE_CQ, q_depth, q_priority, i); mcq_q_cfg->sent_cmd_count[i] = 0; } // check if tag is enough for (i = 0, cnt = 0; i < sq_nr; i++) { queue = &mcq_q_cfg->sq[i]; cnt += queue->q_depth; if (cnt > UFSHCD_MAX_TAG) goto err; } for (i = 0, cnt = 0; i < cq_nr; i++) { queue = &mcq_q_cfg->cq[i]; cnt += queue->q_depth; if (cnt > UFSHCD_MAX_TAG) goto err; } mcq_q_cfg->sq_nr = sq_nr; mcq_q_cfg->cq_nr = cq_nr; dev_info(hba->dev, "SQ NR = %d\n", mcq_q_cfg->sq_nr); dev_info(hba->dev, "CQ NR = %d\n", mcq_q_cfg->cq_nr); return 0; err: dev_err(hba->dev, "MCQ configuration not applied. ONLY %d tag is allowed", UFSHCD_MAX_TAG); return -EINVAL; } static void ufs_mtk_mcq_host_memory_configure(struct ufs_hba *hba) { int i; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; u8 q_size = hba_priv->mcq_nr_hw_queue; u16 sq_offset = 0; u16 sq_offset_1k = 0; u16 cq_offset = 0; u16 cq_offset_1k = 0; struct ufs_queue *sq_ptr; struct ufs_queue *cq_ptr; for (i = 0; i < q_size; i++) { sq_ptr = &hba_priv->mcq_q_cfg.sq[i]; sq_ptr->q_base_addr = (void *)hba_priv->usel_base_addr + sq_offset_1k * SQE_SIZE; sq_ptr->q_dma_addr = hba_priv->usel_dma_addr + sq_offset_1k * SQE_SIZE; sq_offset += sq_ptr->q_depth; sq_offset_1k += DIV_ROUND_UP(sq_ptr->q_depth, SQE_NUM_1K) * SQE_NUM_1K; //for 1K alignment constraint spin_lock_init(&(sq_ptr->q_lock)); dev_info(hba->dev, "SQ[%d], depth: %d, sq_offset: %d, sq_offset_1k: %d", i, sq_ptr->q_depth, sq_offset, sq_offset_1k); } for (i = 0; i < q_size; i++) { cq_ptr = &hba_priv->mcq_q_cfg.cq[i]; cq_ptr->q_base_addr = (void *)hba_priv->ucel_base_addr + cq_offset_1k * CQE_SIZE; cq_ptr->q_dma_addr = hba_priv->ucel_dma_addr + cq_offset_1k * CQE_SIZE; cq_offset += cq_ptr->q_depth; cq_offset_1k += DIV_ROUND_UP(cq_ptr->q_depth, CQE_NUM_1K) * CQE_NUM_1K; //for 1K alignment constraint spin_lock_init(&(cq_ptr->q_lock)); dev_info(hba->dev, "CQ[%d], depth: %d, cq_offset: %d, cq_offset_1k: %d", i, cq_ptr->q_depth, cq_offset, cq_offset_1k); } } static inline void ufs_mtk_mcq_reset_ptr(struct ufs_queue *q) { q->head = q->tail = q->tail_written = q->q_base_addr; } static void ufs_mtk_mcq_enable(struct ufs_hba *hba) { int i; u32 cq_ie, sq_ie; u32 val; u64 addr; struct ufs_queue *q; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; // enable AH8 + MCQ ufshcd_rmwl(hba, MCQ_AH8, MCQ_AH8, REG_UFS_MMIO_OPT_CTRL_0); // set queue type: MCQ ufshcd_rmwl(hba, MCQCFG_TYPE, MCQCFG_TYPE, REG_UFS_MCQCFG); // set arbitration scheme, check capability SP & RRP ufshcd_rmwl(hba, MCQCFG_ARB_SCHEME, MCQCFG_ARB_RRP, REG_UFS_MCQCFG); cq_ie = 0; for (i = 0; i < hba_priv->mcq_nr_hw_queue; i++) { q = &hba_priv->mcq_q_cfg.cq[i]; if (q->q_enable) { val = (q->q_depth * CQE_SIZE / 4) & 0xffff; //Q_SIZE in unit of DWORD val |= Q_ENABLE; addr = q->q_dma_addr; cq_ie |= (1 << i); } else { val = 0; addr = 0; } ufshcd_writel(hba, lower_32_bits(addr), MCQ_ADDR(REG_UFS_CQ_LBA, i)); ufshcd_writel(hba, upper_32_bits(addr), MCQ_ADDR(REG_UFS_CQ_UBA, i)); ufshcd_writel(hba, val, MCQ_ADDR(REG_UFS_CQ_ATTR, i)); ufs_mtk_mcq_reset_ptr(q); } sq_ie = 0; for (i = 0; i < hba_priv->mcq_nr_hw_queue; i++) { q = &hba_priv->mcq_q_cfg.sq[i]; if (q->q_enable) { val = (q->q_depth * SQE_SIZE / 4) & 0xffff; //Q_SIZE in unit of DWORD val |= (hba_priv->mcq_q_cfg.sq_cq_map[i] << 16); val |= (q->priority << 28); val |= Q_ENABLE; addr = q->q_dma_addr; sq_ie |= (1 << i); } else { val = 0; addr = 0; } ufshcd_writel(hba, lower_32_bits(addr), MCQ_ADDR(REG_UFS_SQ_LBA, i)); ufshcd_writel(hba, upper_32_bits(addr), MCQ_ADDR(REG_UFS_SQ_UBA, i)); ufshcd_writel(hba, val, MCQ_ADDR(REG_UFS_SQ_ATTR, i)); ufs_mtk_mcq_reset_ptr(q); } ufshcd_writel(hba, cq_ie, REG_UFS_MMIO_CQ_IE); /* Enable SQ interrupt */ //ufshcd_writel(hba, sq_ie, REG_UFS_MMIO_SQ_IE); if (hba_priv->mcq_nr_intr > 0) ufshcd_rmwl(hba, MCQ_INTR_EN_MSK, MCQ_MULTI_INTR_EN, REG_UFS_MMIO_OPT_CTRL_0); } static void ufs_mtk_mcq_enable_intr(struct ufs_hba *hba, u32 intrs) { u32 set = ufshcd_readl(hba, REG_INTERRUPT_ENABLE); if (hba->ufs_version == ufshci_version(1, 0)) { u32 rw; rw = set & INTERRUPT_MASK_RW_VER_10; set = rw | ((set ^ intrs) & intrs); } else { set |= intrs; } ufshcd_writel(hba, set, REG_INTERRUPT_ENABLE); } static void ufs_mtk_mcq_make_hba_operational(void *data, struct ufs_hba *hba, int *err) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; u32 reg; *err = 0; if (hba_priv->mcq_nr_intr > 0) ufs_mtk_mcq_enable_intr(hba, UFSHCD_ENABLE_INTRS_MCQ_SEPARATE); else ufs_mtk_mcq_enable_intr(hba, UFSHCD_ENABLE_INTRS_MCQ); ufs_mtk_mcq_enable(hba); /* Configure UTMRL base address registers */ ufshcd_writel(hba, lower_32_bits(hba->utmrdl_dma_addr), REG_UTP_TASK_REQ_LIST_BASE_L); ufshcd_writel(hba, upper_32_bits(hba->utmrdl_dma_addr), REG_UTP_TASK_REQ_LIST_BASE_H); /* * Make sure base address and interrupt setup are updated before * enabling the run/stop registers below. */ wmb(); /* * UCRDY, UTMRLDY and UTRLRDY bits must be 1 */ reg = ufshcd_readl(hba, REG_CONTROLLER_STATUS); if ((reg & UFSHCD_STATUS_READY) == UFSHCD_STATUS_READY) { ufshcd_writel(hba, UTP_TASK_REQ_LIST_RUN_STOP_BIT, REG_UTP_TASK_REQ_LIST_RUN_STOP); } else { dev_err(hba->dev, "Host controller not ready to process requests"); *err = -EIO; } } static void ufs_mtk_mcq_use_mcq_hooks(void *data, struct ufs_hba *hba, bool *use_mcq) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; *use_mcq = hba_priv->is_mcq_enabled; } static void ufs_mtk_mcq_hba_capabilities(void *data, struct ufs_hba *hba, int *err) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int nr; if (!hba_priv->is_mcq_enabled) return; hba->reserved_slot = UFSHCD_MAX_TAG - 1; /* Get MCQ number */ hba_priv->mcq_cap = ufshcd_readl(hba, REG_UFS_MCQCAP); hba_priv->max_q = FIELD_GET(MAX_Q, hba_priv->mcq_cap); /* Sanity check */ if (hba_priv->mcq_nr_hw_queue > hba_priv->max_q) { dev_err(hba->dev, "HCI maxq %d is less than %d\n", hba_priv->max_q, hba_priv->mcq_nr_hw_queue); *err = -EINVAL; return; } if (hba_priv->mcq_nr_hw_queue == 0) { dev_err(hba->dev, "Enable MCQ but set hw queue to zero\n"); *err = -EINVAL; return; } nr = hba_priv->mcq_nr_hw_queue * hba_priv->mcq_nr_q_depth; if (nr > UFSHCD_MAX_TAG) { dev_err(hba->dev, "MCQ tag %d is larger than %d\n", nr, UFSHCD_MAX_TAG); *err = -EINVAL; return; } } static void ufs_mtk_mcq_print_evt(struct ufs_hba *hba, u32 id, char *err_name) { int i; bool found = false; struct ufs_event_hist *e; if (id >= UFS_EVT_CNT) return; e = &hba->ufs_stats.event[id]; for (i = 0; i < UFS_EVENT_HIST_LENGTH; i++) { int p = (i + e->pos) % UFS_EVENT_HIST_LENGTH; if (e->tstamp[p] == 0) continue; dev_err(hba->dev, "%s[%d] = 0x%x at %lld us\n", err_name, p, e->val[p], ktime_to_us(e->tstamp[p])); found = true; } if (!found) dev_err(hba->dev, "No record of %s\n", err_name); else dev_err(hba->dev, "%s: total cnt=%llu\n", err_name, e->cnt); } static void ufs_mtk_mcq_print_evt_hist(struct ufs_hba *hba) { ufshcd_dump_regs(hba, 0, UFSHCI_REG_SPACE_SIZE, "host_regs: "); ufs_mtk_mcq_print_evt(hba, UFS_EVT_PA_ERR, "pa_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_DL_ERR, "dl_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_NL_ERR, "nl_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_TL_ERR, "tl_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_DME_ERR, "dme_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_AUTO_HIBERN8_ERR, "auto_hibern8_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_FATAL_ERR, "fatal_err"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_LINK_STARTUP_FAIL, "link_startup_fail"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_RESUME_ERR, "resume_fail"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_SUSPEND_ERR, "suspend_fail"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_DEV_RESET, "dev_reset"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_HOST_RESET, "host_reset"); ufs_mtk_mcq_print_evt(hba, UFS_EVT_ABORT, "task_abort"); ufshcd_vops_dbg_register_dump(hba); } static void ufs_mtk_mcq_print_clk_freqs(struct ufs_hba *hba) { struct ufs_clk_info *clki; struct list_head *head = &hba->clk_list_head; if (list_empty(head)) return; list_for_each_entry(clki, head, list) { if (!IS_ERR_OR_NULL(clki->clk) && clki->min_freq && clki->max_freq) dev_err(hba->dev, "clk: %s, rate: %u\n", clki->name, clki->curr_freq); } } static void ufs_mtk_mcq_print_host_state(struct ufs_hba *hba) { struct scsi_device *sdev_ufs = hba->sdev_ufs_device; dev_err(hba->dev, "UFS Host state=%d\n", hba->ufshcd_state); dev_err(hba->dev, "outstanding reqs=0x%lx tasks=0x%lx\n", hba->outstanding_reqs, hba->outstanding_tasks); dev_err(hba->dev, "saved_err=0x%x, saved_uic_err=0x%x\n", hba->saved_err, hba->saved_uic_err); dev_err(hba->dev, "Device power mode=%d, UIC link state=%d\n", hba->curr_dev_pwr_mode, hba->uic_link_state); dev_err(hba->dev, "PM in progress=%d, sys. suspended=%d\n", hba->pm_op_in_progress, hba->is_sys_suspended); dev_err(hba->dev, "Auto BKOPS=%d, Host self-block=%d\n", hba->auto_bkops_enabled, hba->host->host_self_blocked); dev_err(hba->dev, "Clk gate=%d\n", hba->clk_gating.state); dev_err(hba->dev, "last_hibern8_exit_tstamp at %lld us, hibern8_exit_cnt=%d\n", ktime_to_us(hba->ufs_stats.last_hibern8_exit_tstamp), hba->ufs_stats.hibern8_exit_cnt); dev_err(hba->dev, "last intr at %lld us, last intr status=0x%x\n", ktime_to_us(hba->ufs_stats.last_intr_ts), hba->ufs_stats.last_intr_status); dev_err(hba->dev, "error handling flags=0x%x, req. abort count=%d\n", hba->eh_flags, hba->req_abort_count); dev_err(hba->dev, "hba->ufs_version=0x%x, Host capabilities=0x%x, caps=0x%x\n", hba->ufs_version, hba->capabilities, hba->caps); dev_err(hba->dev, "quirks=0x%x, dev. quirks=0x%x\n", hba->quirks, hba->dev_quirks); if (sdev_ufs) dev_err(hba->dev, "UFS dev info: %.8s %.16s rev %.4s\n", sdev_ufs->vendor, sdev_ufs->model, sdev_ufs->rev); ufs_mtk_mcq_print_clk_freqs(hba); } static void ufs_mtk_mcq_print_pwr_info(struct ufs_hba *hba) { static const char * const names[] = { "INVALID MODE", "FAST MODE", "SLOW_MODE", "INVALID MODE", "FASTAUTO_MODE", "SLOWAUTO_MODE", "INVALID MODE", }; /* * Using dev_dbg to avoid messages during runtime PM to avoid * never-ending cycles of messages written back to storage by user space * causing runtime resume, causing more messages and so on. */ dev_dbg(hba->dev, "%s:[RX, TX]: gear=[%d, %d], lane[%d, %d], pwr[%s, %s], rate = %d\n", __func__, hba->pwr_info.gear_rx, hba->pwr_info.gear_tx, hba->pwr_info.lane_rx, hba->pwr_info.lane_tx, names[hba->pwr_info.pwr_rx], names[hba->pwr_info.pwr_tx], hba->pwr_info.hs_rate); } static inline bool ufs_mtk_mcq_is_saved_err_fatal(struct ufs_hba *hba) { return (hba->saved_uic_err & UFSHCD_UIC_DL_PA_INIT_ERROR) || (hba->saved_err & (INT_FATAL_ERRORS | UFSHCD_UIC_HIBERN8_MASK)); } static inline void ufs_mtk_mcq_schedule_eh_work(struct ufs_hba *hba) { /* handle fatal errors only when link is not in error state */ if (hba->ufshcd_state != UFSHCD_STATE_ERROR) { if (hba->force_reset || ufshcd_is_link_broken(hba) || ufs_mtk_mcq_is_saved_err_fatal(hba)) hba->ufshcd_state = UFSHCD_STATE_EH_SCHEDULED_FATAL; else hba->ufshcd_state = UFSHCD_STATE_EH_SCHEDULED_NON_FATAL; queue_work(hba->eh_wq, &hba->eh_work); } } static void ufs_mtk_mcq_set_req_abort_skip(struct ufs_hba *hba, unsigned long *bitmap) { struct ufshcd_lrb *lrbp; int tag; for_each_set_bit(tag, bitmap, UFSHCD_MAX_TAG) { lrbp = &hba->lrb[tag]; lrbp->req_abort_skip = true; } } static int ufs_mtk_try_to_abort_task(struct ufs_hba *hba, int tag) { /* Not implement MCQ stop and clean command * If implement MCQ stop and clean, need export ufshcd_issue_tm_cmd() * to call UFS_QUERY_TASK and UFS_ABORT_TASK */ return 0; } static void ufs_mtk_mcq_abort(void *data, struct scsi_cmnd *cmd, int *ret) { struct Scsi_Host *host = cmd->device->host; struct ufs_hba *hba = shost_priv(host); struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int tag = scsi_cmd_to_rq(cmd)->tag; struct ufshcd_lrb *lrbp; unsigned long flags; int err; *ret = FAILED; ufs_mtk_mcq_map_tag(NULL, hba, scsi_cmd_to_rq(cmd)->mq_hctx->queue_num, &tag); lrbp = &hba->lrb[tag]; ufshcd_hold(hba, false); /* If command is already aborted/completed, return FAILED. */ if (!(test_bit(tag, hba_priv->outstanding_mcq_reqs))) { dev_err(hba->dev, "%s: cmd at tag %d already completed, outstanding=0x%lx\n", __func__, tag, hba->outstanding_reqs); goto release; } /* Print Transfer Request of aborted task */ dev_info(hba->dev, "%s: Deivce abort at hwq:%d, tag:%d", __func__, scsi_cmd_to_rq(cmd)->mq_hctx->queue_num, tag); /* * Print detailed info about aborted request. * As more than one request might get aborted at the same time, * print full information only for the first aborted request in order * to reduce repeated printouts. For other aborted requests only print * basic details. */ scsi_print_command(cmd); if (!hba->req_abort_count) { ufshcd_update_evt_hist(hba, UFS_EVT_ABORT, tag); ufs_mtk_mcq_print_evt_hist(hba); ufs_mtk_mcq_print_host_state(hba); ufs_mtk_mcq_print_pwr_info(hba); ufs_mtk_mcq_print_trs_tag(hba, tag, true); } else { ufs_mtk_mcq_print_trs_tag(hba, tag, false); } hba->req_abort_count++; /* * Task abort to the device W-LUN is illegal. When this command * will fail, due to spec violation, scsi err handling next step * will be to send LU reset which, again, is a spec violation. * To avoid these unnecessary/illegal steps, first we clean up * the lrb taken by this cmd and re-set it in outstanding_reqs, * then queue the eh_work and bail. */ if (lrbp->lun == UFS_UPIU_UFS_DEVICE_WLUN) { ufshcd_update_evt_hist(hba, UFS_EVT_ABORT, lrbp->lun); spin_lock_irqsave(host->host_lock, flags); hba->force_reset = true; ufs_mtk_mcq_schedule_eh_work(hba); spin_unlock_irqrestore(host->host_lock, flags); goto release; } /* Skip task abort in case previous aborts failed and report failure */ if (lrbp->req_abort_skip) { dev_err(hba->dev, "%s: skipping abort\n", __func__); ufs_mtk_mcq_set_req_abort_skip(hba, hba_priv->outstanding_mcq_reqs); goto release; } err = ufs_mtk_try_to_abort_task(hba, tag); if (err) { dev_err(hba->dev, "%s: failed with err %d\n", __func__, err); ufs_mtk_mcq_set_req_abort_skip(hba, hba_priv->outstanding_mcq_reqs); *ret = FAILED; goto release; } lrbp->cmd = NULL; /* No clean SQ */ /* *ret = SUCCESS; */ release: /* Matches the ufshcd_hold() call at the start of this function. */ ufshcd_release(hba); return; } static void ufs_mtk_mcq_config(void *data, struct ufs_hba *hba, int *err) { struct Scsi_Host *host = hba->host; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; *err = ufs_mtk_mcq_init_queue(hba); if (*err) return; ufs_mtk_mcq_host_memory_configure(hba); /* Reserve one depth to judge empty or full */ host->can_queue = hba_priv->mcq_nr_q_depth - UFSHCD_MCQ_NUM_RESERVED; host->cmd_per_lun = hba_priv->mcq_nr_q_depth - UFSHCD_MCQ_NUM_RESERVED; host->nr_hw_queues = hba_priv->mcq_nr_hw_queue; } static void ufs_mtk_mcq_max_tag(void *data, struct ufs_hba *hba, int *max_tag) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; if (hba_priv->is_mcq_enabled) *max_tag = UFSHCD_MAX_TAG; else *max_tag = hba->nutrs; } static void ufs_mtk_mcq_has_oustanding_reqs(void *data, struct ufs_hba *hba, bool *ret) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int i; if (!hba_priv->is_mcq_enabled) { *ret = hba->outstanding_reqs != 0 ? true : false; return; } for (i = 0; i < BITMAP_TAGS_LEN; i++) { if (hba_priv->outstanding_mcq_reqs[i]) { *ret = true; return; } } *ret = false; } static void ufs_mtk_mcq_get_outstanding_reqs(void *data, struct ufs_hba *hba, unsigned long **outstanding_reqs, int *nr_tag) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; if (hba_priv->is_mcq_enabled) { *outstanding_reqs = hba_priv->outstanding_mcq_reqs; if (nr_tag) *nr_tag = UFSHCD_MAX_TAG; } else { *outstanding_reqs = &hba->outstanding_reqs; if (nr_tag) *nr_tag = hba->nutrs; } } static void ufs_mtk_mcq_clear_cmd(void *data, struct ufs_hba *hba, int tag, int *ret) { // Mediatek MCQ not imiplement SQ stop and clean *ret = FAILED; } static void ufs_mtk_mcq_clear_pending(void *data, struct ufs_hba *hba, int *ret) { // Mediatek MCQ not imiplement SQ stop and clean *ret = FAILED; } static void ufs_mtk_mcq_map_tag(void *data, struct ufs_hba *hba, int index, int *tag) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int oldtag = *tag; if (!hba_priv->is_mcq_enabled) return; *tag = oldtag + index * hba_priv->mcq_nr_q_depth; } static void ufs_mtk_mcq_set_sqid(void *data, struct ufs_hba *hba, int index, struct ufshcd_lrb *lrbp) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; if (!hba_priv->is_mcq_enabled) return; /* Set hwq index to lrb */ lrbp->android_vendor_data1 = index; } static void ufs_mtk_mcq_retry_complete(void *data, struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int index; unsigned long flags; if (!hba_priv->is_mcq_enabled) return; spin_lock_irqsave(&hba->outstanding_lock, flags); for_each_set_bit(index, hba_priv->outstanding_mcq_reqs, UFSHCD_MAX_TAG) { ufs_mtk_transfer_req_compl_handler(hba, true, index); } bitmap_zero(hba_priv->outstanding_mcq_reqs, UFSHCD_MAX_TAG); spin_unlock_irqrestore(&hba->outstanding_lock, flags); } int ufs_mtk_mcq_alloc_priv(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv; hba_priv = kzalloc(sizeof(struct ufs_hba_private), GFP_KERNEL); if (!hba_priv) { dev_err(hba->dev, "Allocate ufs private struct fail\n"); return -1; } hba->android_vendor_data1 = (u64)hba_priv; return 0; } void ufs_mtk_mcq_host_dts(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct device_node *np = hba->dev->of_node; #if IS_ENABLED(CONFIG_UFS_MEDIATEK_INTERNAL) struct tag_chipid *chipid; unsigned int val; #endif int i; if (of_property_read_bool(np, "mediatek,ufs-mcq-enabled")) hba_priv->is_mcq_enabled = true; else hba_priv->is_mcq_enabled = false; #if IS_ENABLED(CONFIG_UFS_MEDIATEK_INTERNAL) /* Get chip id from bootmode */ chipid = (struct tag_chipid *)ufs_mtk_get_boot_property(np, "atag,chipid", NULL); if (chipid && !of_property_read_u32(np, "mediatek,ufs-mcq-disable-on-hwver", &val)) { dev_dbg(hba->dev, "%s: hwver=0x%x\n", __func__, chipid->hw_ver); if (chipid->hw_ver == val) { hba_priv->is_mcq_enabled = false; dev_info(hba->dev, "mcq disabled"); } } #endif if (hba_priv->is_mcq_enabled) { hba_priv->mcq_nr_hw_queue = 1; of_property_read_u32(np, "mediatek,ufs-mcq-hwq-count", &hba_priv->mcq_nr_hw_queue); hba_priv->mcq_nr_q_depth = 32; of_property_read_u32(np, "mediatek,ufs-mcq-q-depth", &hba_priv->mcq_nr_q_depth); hba_priv->mcq_nr_intr = 0; if (of_property_read_u32(np, "mediatek,ufs-mcq-intr-count", &hba_priv->mcq_nr_intr) == 0) { if (hba_priv->mcq_nr_intr > 0) { for (i = 0; i < hba_priv->mcq_nr_intr; i++) { hba_priv->mcq_intr_info[i].qid = i; hba_priv->mcq_intr_info[i].intr = mtk_mcq_irq[i]; hba_priv->mcq_intr_info[i].hba = hba; dev_info(hba->dev, "Set MCQ interrupt: %d, %d\n", i, hba_priv->mcq_intr_info[i].intr); } } } dev_info(hba->dev, "%s, mcq hwq: %d, mcq q-depth: %d, mcq_nr_intr: %d\n", __func__, hba_priv->mcq_nr_hw_queue, hba_priv->mcq_nr_q_depth, hba_priv->mcq_nr_intr); } dev_info(hba->dev, "MCQ enabled: %s\n", hba_priv->is_mcq_enabled ? "yes" : "no"); } void ufs_mtk_mcq_get_irq(struct platform_device *pdev) { int i, irq; for (i = 0; i < UFSHCD_MAX_Q_NR; i++) mtk_mcq_irq[i] = MTK_MCQ_INVALID_IRQ; for (i = 0; i < UFSHCD_MAX_Q_NR; i++) { /* irq index 0 is ufshcd system irq, sq, cq irq start from index 1 */ irq = platform_get_irq(pdev, i + 1); if (irq < 0) { dev_info(&pdev->dev, "Get platform interrupt fail: %d\n", i); break; } mtk_mcq_irq[i] = irq; dev_info(&pdev->dev, "Get platform interrupt: %d, %d\n", i, irq); } } void ufs_mtk_mcq_request_irq(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; unsigned int irq, i; int ret; if (!hba_priv->is_mcq_enabled) return; if (!hba_priv->mcq_nr_intr) return; // Disable irq option register ufshcd_rmwl(hba, MCQ_INTR_EN_MSK, 0, REG_UFS_MMIO_OPT_CTRL_0); for (i = 0; i < hba_priv->mcq_nr_intr; i++) { irq = hba_priv->mcq_intr_info[i].intr; if (irq == MTK_MCQ_INVALID_IRQ) { dev_info(hba->dev, "mcq_intr: %d is invalid\n", i); break; } ret = devm_request_irq(hba->dev, irq, ufs_mtk_mcq_intr, 0, UFSHCD, &hba_priv->mcq_intr_info[i]); if (ret) { dev_err(hba->dev, "request irq %d failed\n", irq); return; } dev_info(hba->dev, "request_irq: %d\n", irq); } } void ufs_mtk_mcq_set_irq_affinity(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct blk_mq_tag_set *tag_set = &hba->host->tag_set; struct blk_mq_queue_map *map = &tag_set->map[HCTX_TYPE_DEFAULT]; unsigned int nr = map->nr_queues; unsigned int q_index, cpu, _cpu, irq; int ret; if (!hba_priv->is_mcq_enabled) return; if (hba_priv->mcq_nr_intr == 0) return; /* Set affinity */ for (cpu = 0; cpu < nr; cpu++) { q_index = map->mq_map[cpu]; irq = hba_priv->mcq_intr_info[q_index].intr; /* force migrate irq of cpu0 to cpu3 */ _cpu = (cpu == 0) ? 3 : cpu; ret = irq_set_affinity(irq, cpumask_of(_cpu)); if (ret) { dev_info(hba->dev, "mcq: irq_set_affinity irq %d on CPU %d failed\n", irq, _cpu); return; } dev_info(hba->dev, "Set irq %d to CPU: %d\n", irq, _cpu); } } void ufs_mtk_mcq_disable_irq(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct blk_mq_tag_set *tag_set = &hba->host->tag_set; struct blk_mq_queue_map *map = &tag_set->map[HCTX_TYPE_DEFAULT]; unsigned int nr = map->nr_queues; unsigned int q_index, cpu, irq; if (!hba_priv->is_mcq_enabled) return; if (hba_priv->mcq_nr_intr == 0) return; for (cpu = 0; cpu < nr; cpu++) { q_index = map->mq_map[cpu]; irq = hba_priv->mcq_intr_info[q_index].intr; disable_irq(irq); } hba_priv->is_mcq_intr_enabled = false; } void ufs_mtk_mcq_enable_irq(struct ufs_hba *hba) { struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; struct blk_mq_tag_set *tag_set = &hba->host->tag_set; struct blk_mq_queue_map *map = &tag_set->map[HCTX_TYPE_DEFAULT]; unsigned int nr = map->nr_queues; unsigned int q_index, cpu, irq; if (!hba_priv->is_mcq_enabled) return; if (hba_priv->mcq_nr_intr == 0) return; /* * Racing by gate work(should turn off clock, but not) and * ufshcd_hold (turn on again). * * ufshcd_gate_work -> spin_lock_irqsave 2 * -> active_reqs !=0 7 * -> spin_unlock_irqrestore 8 * ufshcd_hold -> spin_lock_irqsave 1 * -> active_reqs ++ 3 * -> cancel_delayed_work fail 4 * -> ungate work (turn clock) 5 * -> spin_unlock_irqrestore 6 */ if (hba_priv->is_mcq_intr_enabled == true) return; for (cpu = 0; cpu < nr; cpu++) { q_index = map->mq_map[cpu]; irq = hba_priv->mcq_intr_info[q_index].intr; enable_irq(irq); } hba_priv->is_mcq_intr_enabled = true; } int ufs_mtk_mcq_memory_alloc(struct ufs_hba *hba) { size_t usel_size, ucel_size; struct ufs_hba_private *hba_priv = (struct ufs_hba_private *)hba->android_vendor_data1; int q_size = hba_priv->mcq_nr_hw_queue; int pool_size; if (!hba_priv->is_mcq_enabled) return 0; pool_size = UFSHCD_MAX_TAG; /* * assume SQi/CQi depth is multiple of 32, and * sum(SQi depth) = sum(CQi depth) = UFSHCD_MAX_TAG * if SQi/CQi may not be multple of 32, need to prepare 8 * SQE_NUM_1K * extra size for the maximum possible fragments */ //SQ entry usel_size = SQE_SIZE * pool_size; hba_priv->usel_base_addr = dmam_alloc_coherent(hba->dev, usel_size, &hba_priv->usel_dma_addr, GFP_KERNEL); if (!hba_priv->usel_base_addr || WARN_ON(hba_priv->usel_dma_addr & (PAGE_SIZE - 1))) { dev_err(hba->dev, "SQ Entry Memory allocation failed\n"); goto out; } //CQ entry ucel_size = CQE_SIZE * pool_size; hba_priv->ucel_base_addr = dmam_alloc_coherent(hba->dev, ucel_size, &hba_priv->ucel_dma_addr, GFP_KERNEL); if (!hba_priv->ucel_base_addr || WARN_ON(hba_priv->ucel_dma_addr & (PAGE_SIZE - 1))) { dev_err(hba->dev, "CQ Entry Memory allocation failed\n"); goto out; } //SQ structure hba_priv->mcq_q_cfg.sq = devm_kcalloc(hba->dev, q_size, sizeof(struct ufs_queue), GFP_KERNEL); //CQ structure hba_priv->mcq_q_cfg.cq = devm_kcalloc(hba->dev, q_size, sizeof(struct ufs_queue), GFP_KERNEL); return 0; out: return -ENOMEM; } struct tracepoints_table { const char *name; void *func; struct tracepoint *tp; bool init; }; static struct tracepoints_table mcq_interests[] = { { .name = "android_vh_ufs_use_mcq_hooks", .func = ufs_mtk_mcq_use_mcq_hooks }, { .name = "android_vh_ufs_mcq_max_tag", .func = ufs_mtk_mcq_max_tag }, { .name = "android_vh_ufs_mcq_map_tag", .func = ufs_mtk_mcq_map_tag }, { .name = "android_vh_ufs_mcq_set_sqid", .func = ufs_mtk_mcq_set_sqid }, { .name = "android_vh_ufs_mcq_handler", .func = ufs_mtk_legacy_mcq_handler }, { .name = "android_vh_ufs_mcq_make_hba_operational", .func = ufs_mtk_mcq_make_hba_operational }, { .name = "android_vh_ufs_mcq_hba_capabilities", .func = ufs_mtk_mcq_hba_capabilities }, { .name = "android_vh_ufs_mcq_print_trs", .func = ufs_mtk_mcq_print_trs }, { .name = "android_vh_ufs_mcq_send_command", .func = ufs_mtk_mcq_send_command }, { .name = "android_vh_ufs_mcq_config", .func = ufs_mtk_mcq_config }, { .name = "android_vh_ufs_mcq_has_oustanding_reqs", .func = ufs_mtk_mcq_has_oustanding_reqs }, { .name = "android_vh_ufs_mcq_get_outstanding_reqs", .func = ufs_mtk_mcq_get_outstanding_reqs }, { .name = "android_vh_ufs_mcq_abort", .func = ufs_mtk_mcq_abort }, { .name = "android_vh_ufs_mcq_clear_cmd", .func = ufs_mtk_mcq_clear_cmd }, { .name = "android_vh_ufs_mcq_clear_pending", .func = ufs_mtk_mcq_clear_pending }, { .name = "android_vh_ufs_mcq_retry_complete", .func = ufs_mtk_mcq_retry_complete }, }; #define FOR_EACH_INTEREST(i) \ for (i = 0; i < sizeof(mcq_interests) / sizeof(struct tracepoints_table); \ i++) static void ufs_mtk_mcq_lookup_tracepoints(struct tracepoint *tp, void *ignore) { int i; FOR_EACH_INTEREST(i) { if (strcmp(mcq_interests[i].name, tp->name) == 0) mcq_interests[i].tp = tp; } } void ufs_mtk_mcq_uninstall_tracepoints(void) { int i; FOR_EACH_INTEREST(i) { if (mcq_interests[i].init) { tracepoint_probe_unregister(mcq_interests[i].tp, mcq_interests[i].func, NULL); } } } int ufs_mtk_mcq_install_tracepoints(void) { int i; /* Install the tracepoints */ for_each_kernel_tracepoint(ufs_mtk_mcq_lookup_tracepoints, NULL); FOR_EACH_INTEREST(i) { if (mcq_interests[i].tp == NULL) { pr_info("Error: tracepoint %s not found\n", mcq_interests[i].name); continue; } tracepoint_probe_register(mcq_interests[i].tp, mcq_interests[i].func, NULL); mcq_interests[i].init = true; } return 0; }