Re: [PATCH v9 3/6] net: wwan: t9xx: Add control DMA interface

diff --git a/drivers/net/wwan/t9xx/pcie/mtk_cldma.c b/drivers/net/wwan/t9xx/pcie/mtk_cldma.c
new file mode 100644
index 0000000000000..0f281bfb38cc7
--- /dev/null
+++ b/drivers/net/wwan/t9xx/pcie/mtk_cldma.c

@@ -0,0 +1,1520 @@

+static int mtk_cldma_rx_skb_adjust(struct mtk_md_dev *mdev, struct rxq *rxq,
+				   struct rx_req *req)
+{

+		recv_len = le16_to_cpu(bd_dsc->bd->rx_bd.data_recv_len);
+		allow_len = le16_to_cpu(bd_dsc->bd->rx_bd.data_allow_len);
+		if (recv_len > allow_len) {
+			ret = -EPROTO;
+			recv_len = allow_len;
+		}
+		total_len += recv_len;
+		if (total_len > req->mtu) {
+			ret = -EPROTO;
+			recv_len -= min(total_len - req->mtu, recv_len);
+			total_len = req->mtu;
+		}
+		bd_dsc->skb->len = 0;
+		skb_reset_tail_pointer(bd_dsc->skb);
+		skb_put(bd_dsc->skb, recv_len);

+			skb_trim(rx_skb, 0);
+			req->skb = rx_skb;
+			req->data_dma_addr = dma_map_single(mdev->dev,
+							    rx_skb->data,
+							    req->mtu,
+							    DMA_FROM_DEVICE);
+			if (dma_mapping_error(mdev->dev, req->data_dma_addr)) {
+				req->data_dma_addr = 0;
+				/* Keep rx_skb in req->skb for clean stall.
+				 * HWO is not set — HW won't touch this slot.
+				 * Queue stalls until modem reset recovery.
+				 */
+				goto out;

+	if (!atomic_read(&rxq->need_exit)) {
+		if (atomic_xchg(&rxq->need_restart, 0))
+			mtk_cldma_rxq_restart(drv_info, rxq);
+		else if (ret != -ENXIO)
+			mtk_cldma_resume_queue(drv_info, DIR_RX, rxq->rxqno);
+	}

+		txq = drv_info->txq[i];
+		if (txq) {
+			spin_lock(&txq->ring_lock);
+			mtk_cldma_setup_start_addr(drv_info, DIR_TX, i,
+						   txq->req_pool[txq->free_idx].gpd_dma_addr);
+			mtk_cldma_unmask_intr(drv_info, DIR_TX, i, QUEUE_ERROR);
+			mtk_cldma_unmask_intr(drv_info, DIR_TX, i, QUEUE_XFER_DONE);
+			if (READ_ONCE(txq->tx_started))
+				mtk_cldma_start_queue(drv_info, DIR_TX, i);
+			spin_unlock(&txq->ring_lock);
+		}

+	ret = mtk_cldma_start_xfer(drv_info, que->txqno);
+	if (unlikely(ret)) {
+		dev_err(mdev->dev, "Failed to trigger cldma tx\n");
+		mtk_cldma_txq_flush(drv_info, txq, ret);
+	}

diff --git a/drivers/net/wwan/t9xx/pcie/mtk_cldma_drv.c b/drivers/net/wwan/t9xx/pcie/mtk_cldma_drv.c
new file mode 100644
index 0000000000000..8ab153da2083e
--- /dev/null
+++ b/drivers/net/wwan/t9xx/pcie/mtk_cldma_drv.c

@@ -0,0 +1,561 @@

+void mtk_cldma_drv_reset(struct cldma_drv_info *drv_info)
+{
+	struct cldma_hw_regs *hw_regs;
+	struct mtk_md_dev *mdev;
+	u32 val;
+
+	mdev = drv_info->mdev;
+	hw_regs = drv_info->hw_regs;
+
+	val = mtk_pci_read32(mdev, REG_DEV_INFRA_BASE + hw_regs->reg_infra_rst0_set);
+	val |= 1 << (REG_CLDMA0_RST_SET_BIT + drv_info->hw_id);
+	mtk_pci_write32(mdev, REG_DEV_INFRA_BASE + hw_regs->reg_infra_rst0_set, val);
+	udelay(1);
+	val = mtk_pci_read32(mdev, REG_DEV_INFRA_BASE + hw_regs->reg_infra_rst0_clr);
+	val |= 1 << (REG_CLDMA0_RST_CLR_BIT + drv_info->hw_id);
+	mtk_pci_write32(mdev, REG_DEV_INFRA_BASE + hw_regs->reg_infra_rst0_clr, val);
+}

diff --git a/drivers/net/wwan/t9xx/pcie/mtk_trans_ctrl.c b/drivers/net/wwan/t9xx/pcie/mtk_trans_ctrl.c
new file mode 100644
index 0000000000000..531940b06e2a9
--- /dev/null
+++ b/drivers/net/wwan/t9xx/pcie/mtk_trans_ctrl.c

@@ -0,0 +1,615 @@

+	switch (trb->cmd) {
+	case TRB_CMD_ENABLE:
+		trb_open_priv = (struct trb_open_priv *)skb->data;
+		trb_open_priv->log_rg_offset = que->log_rg_offset;
+		trans->usr_cnt[que->hif_id][que->txqno]++;
+		if (trans->usr_cnt[que->hif_id][que->txqno] == 1)
+			break;
+		trb_open_priv->tx_mtu = que->tx_mtu;
+		trb_open_priv->rx_mtu = que->rx_mtu;
+		trb_open_priv->tx_frag_size = que->tx_frag_size;
+		trb_open_priv->rx_frag_size = que->rx_frag_size;
+		if (mtk_cldma_check_ch_cfg(trans->dev, que)) {
+			trb->status = -EINVAL;
+			ret = -EINVAL;
+		} else {

+		case TRB_CMD_TX:
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			err = mtk_cldma_submit_tx(trans->dev, skb);

+			trans_list->tx_burst_cnt[qno]++;
+			spin_lock_irqsave(&skb_list->lock, flags);
+			if (trans_list->tx_burst_cnt[qno] >= TX_BURST_MAX_CNT ||
+			    skb_queue_is_last(skb_list, skb)) {
+				kick = true;
+			} else {
+				skb_next = skb_peek_next(skb, skb_list);
+				trb_next = (struct trb *)skb_next->cb;
+				if (trb_next->cmd != TRB_CMD_TX)
+					kick = true;
+			}
+
+			__skb_unlink(skb, skb_list);
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			break;

+	mtk_ctrl_chs_flush(srv);
+	return 0;
+}

+int mtk_pcie_hif_exit(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+	struct mtk_ctrl_trans *trans;
+
+	trans = ctrl_blk->ctrl_hw_priv;
+
+	mutex_lock(&trans->submit_lock);
+	atomic_set(&trans->available, 0);
+	mutex_unlock(&trans->submit_lock);
+
+	/* Join the service threads before freeing what they dereference: they
+	 * read trans->dev without a lock, so clearing it cannot stop a
+	 * consumer that has already loaded the pointer.
+	 */
+	mtk_ctrl_trb_srv_exit(trans);
+	mtk_cldma_exit(trans);

+	if (trb->cmd == TRB_CMD_DISABLE) {
+		struct sk_buff *entry = NULL;
+		struct sk_buff_head *list;
+		struct sk_buff *iter;
+		unsigned long flags;
+
+		/* A disable may overtake queued data, so teardown does not
+		 * wait for a TX backlog, but it must never overtake a pending
+		 * ENABLE for the same queue: the two do not commute, and a
+		 * DISABLE consumed before its ENABLE closes nothing while the
+		 * ENABLE then arms rings nobody owns.
+		 */
+		list = &trans->trans_list[que->hif_id].skb_list[que->txqno];
+		spin_lock_irqsave(&list->lock, flags);
+		skb_queue_walk(list, iter) {
+			if (((struct trb *)iter->cb)->cmd != TRB_CMD_ENABLE) {
+				entry = iter;
+				break;
+			}
+		}
+		if (entry)
+			__skb_queue_before(list, entry, skb);
+		else
+			__skb_queue_tail(list, skb);

@@ -0,0 +1,615 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#include <linux/device.h>
+#include <linux/freezer.h>
+#include <linux/hashtable.h>
+#include <linux/kthread.h>
+#include <linux/list.h>
+#include <linux/nospec.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#include "mtk_cldma.h"
+#include "mtk_ctrl_plane.h"
+#include "mtk_dev.h"
+#include "mtk_pci.h"
+#include "mtk_trans_ctrl.h"
+
+#define QUEUE_CHL_MASK	0xFFFF
+#define TRB_SRV_NUM	(1)
+
+static const int mtk_srv_cfg[NR_CLDMA][HW_QUE_NUM] = {
+	{0},
+	{0},
+};
+
+/* the number of RX GPDs should be at least two */
+static const struct queue_info mtk_queue_info[] = {
+	{CCCI_CONTROL_TX, CCCI_CONTROL_RX, CLDMA1, TXQ(0), RXQ(0),
+	 Q_MTU_3_5K, Q_MTU_3_5K, TX_GPD_NUM, RX_GPD_NUM, Q_FRAG_3_5K, Q_FRAG_3_5K, 0},
+	{CCCI_SAP_CONTROL_TX, CCCI_SAP_CONTROL_RX, CLDMA0, TXQ(0), RXQ(0),
+	 Q_MTU_3_5K, Q_MTU_3_5K, TX_GPD_NUM, RX_GPD_NUM, Q_FRAG_3_5K, Q_FRAG_3_5K, 0},
+};
+
+static bool mtk_queue_list_is_full(struct mtk_ctrl_trans *trans, struct queue_info *que)
+{
+	return skb_queue_len_lockless(&trans->trans_list[que->hif_id].skb_list[que->txqno]) >=
+	       SKB_LIST_MAX_LEN;
+}
+
+static bool mtk_ctrl_chs_is_busy_or_empty(struct trb_srv *srv)
+{
+	struct srv_que *srv_que;
+	int i;
+
+	for (i = 0; i < NR_CLDMA; i++) {
+		list_for_each_entry(srv_que, &srv->srv_q_list[i], list) {
+			struct sk_buff *skb;
+			struct trb *trb;
+
+			skb = skb_peek(&srv->trans->trans_list[i].skb_list[srv_que->qno]);
+			if (!skb)
+				continue;
+
+			/* ENABLE and DISABLE are software-only and are queued at
+			 * the head, so gating them on TX budget would make a queue
+			 * that cannot drain impossible to close.
+			 */
+			trb = (struct trb *)skb->cb;
+			if (trb->cmd != TRB_CMD_TX ||
+			    mtk_cldma_get_tx_budget(srv->trans->dev, i, srv_que->qno))
+				return false;
+		}
+	}
+
+	return true;
+}
+
+static void mtk_ctrl_ch_flush(struct sk_buff_head *skb_list)
+{
+	struct sk_buff *skb;
+	struct trb *trb;
+
+	while (!skb_queue_empty(skb_list)) {
+		skb = skb_dequeue(skb_list);
+		trb = (struct trb *)skb->cb;
+		trb->status = -EIO;
+		trb->trb_complete(skb);
+	}
+}
+
+static void mtk_ctrl_chs_flush(struct trb_srv *srv)
+{
+	struct srv_que *srv_que;
+	int i;
+
+	for (i = 0; i < NR_CLDMA; i++)
+		list_for_each_entry(srv_que, &srv->srv_q_list[i], list)
+			mtk_ctrl_ch_flush(&srv->trans->trans_list[i].skb_list[srv_que->qno]);
+}
+
+static int mtk_ch_status_check(struct mtk_ctrl_trans *trans, struct sk_buff *skb)
+{
+	struct trb *trb = (struct trb *)skb->cb;
+	struct trb_open_priv *trb_open_priv;
+	struct queue_info *que;
+	int ret = 0;
+
+	que = radix_tree_lookup(&trans->queue_tbl, trb->channel_id & QUEUE_CHL_MASK);
+
+	switch (trb->cmd) {
+	case TRB_CMD_ENABLE:
+		trb_open_priv = (struct trb_open_priv *)skb->data;
+		trb_open_priv->log_rg_offset = que->log_rg_offset;
+		trans->usr_cnt[que->hif_id][que->txqno]++;
+		if (trans->usr_cnt[que->hif_id][que->txqno] == 1)
+			break;
+		trb_open_priv->tx_mtu = que->tx_mtu;
+		trb_open_priv->rx_mtu = que->rx_mtu;
+		trb_open_priv->tx_frag_size = que->tx_frag_size;
+		trb_open_priv->rx_frag_size = que->rx_frag_size;
+		if (mtk_cldma_check_ch_cfg(trans->dev, que)) {
+			trb->status = -EINVAL;
+			ret = -EINVAL;
+		} else {
+			trb->status = -EBUSY;
+			ret = -EBUSY;
+		}
+		trb->trb_complete(skb);
+		break;
+	case TRB_CMD_DISABLE:
+		if (trans->usr_cnt[que->hif_id][que->txqno] > 0) {
+			trans->usr_cnt[que->hif_id][que->txqno]--;
+			if (!trans->usr_cnt[que->hif_id][que->txqno])
+				break;
+		}
+		trb->status = -EBUSY;
+		trb->trb_complete(skb);
+		ret = -EBUSY;
+		break;
+	default:
+		dev_err((trans->mdev)->dev, "Invalid trb command(%d)\n", trb->cmd);
+		ret = -EINVAL;
+		break;
+	}
+	return ret;
+}
+
+/* Single consumer per srv_que — only this kthread dequeues from skb_list.
+ * The list lock is held around every list read (peek, is_last, peek_next,
+ * unlink) so a producer inserting a DISABLE at the head cannot race the
+ * traversal, but it is dropped across submit and dispatch: those paths
+ * may allocate with GFP_KERNEL and thus sleep. Dropping the lock there
+ * is safe because no other consumer can steal the peeked skb.
+ */
+static void mtk_ctrl_trb_handler(struct trb_srv *srv, struct trans_list *trans_list, u32 qno)
+{
+	struct sk_buff_head *skb_list = &trans_list->skb_list[qno];
+	struct mtk_ctrl_trans *trans = srv->trans;
+	struct sk_buff *skb, *skb_next;
+	struct trb *trb, *trb_next;
+	unsigned long flags;
+	bool kick = false;
+	int loop = 0;
+	int err;
+
+	do {
+		spin_lock_irqsave(&skb_list->lock, flags);
+		skb = skb_peek(skb_list);
+		if (!skb) {
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			break;
+		}
+		trb = (struct trb *)skb->cb;
+
+		switch (trb->cmd) {
+		case TRB_CMD_ENABLE:
+		case TRB_CMD_DISABLE:
+			__skb_unlink(skb, skb_list);
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			err = mtk_ch_status_check(trans, skb);
+			if (!err) {
+				kick = true;
+				if (trb->cmd == TRB_CMD_DISABLE)
+					mtk_ctrl_ch_flush(skb_list);
+			}
+			break;
+		case TRB_CMD_TX:
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			err = mtk_cldma_submit_tx(trans->dev, skb);
+			if (err) {
+				if (trans_list->tx_burst_cnt[qno]) {
+					kick = true;
+					break;
+				}
+				if (err == -EAGAIN)
+					return;
+
+				skb_unlink(skb, skb_list);
+				trb->status = err;
+				trb->trb_complete(skb);
+				break;
+			}
+
+			trans_list->tx_burst_cnt[qno]++;
+			spin_lock_irqsave(&skb_list->lock, flags);
+			if (trans_list->tx_burst_cnt[qno] >= TX_BURST_MAX_CNT ||
+			    skb_queue_is_last(skb_list, skb)) {
+				kick = true;
+			} else {
+				skb_next = skb_peek_next(skb, skb_list);
+				trb_next = (struct trb *)skb_next->cb;
+				if (trb_next->cmd != TRB_CMD_TX)
+					kick = true;
+			}
+
+			__skb_unlink(skb, skb_list);
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			break;
+		default:
+			__skb_unlink(skb, skb_list);
+			spin_unlock_irqrestore(&skb_list->lock, flags);
+			trb->status = -EINVAL;
+			trb->trb_complete(skb);
+			break;
+		}
+
+		if (kick) {
+			err = mtk_cldma_trb_process(trans->dev, skb);
+			if (err)
+				dev_err_ratelimited((trans->mdev)->dev,
+						    "Failed to process trb on queue %u: %d\n",
+						    qno, err);
+			trans_list->tx_burst_cnt[qno] = 0;
+			kick = false;
+		}
+
+		loop++;
+	} while (loop < TRB_NUM_PER_ROUND);
+}
+
+static void mtk_ctrl_trb_process(struct trb_srv *srv)
+{
+	struct mtk_ctrl_trans *trans = srv->trans;
+	struct srv_que *srv_que;
+	int i;
+
+	for (i = 0; i < NR_CLDMA; i++)
+		list_for_each_entry(srv_que, &srv->srv_q_list[i], list)
+			mtk_ctrl_trb_handler(srv, &trans->trans_list[i], srv_que->qno);
+}
+
+static int mtk_ctrl_trb_thread(void *args)
+{
+	struct trb_srv *srv = args;
+
+	for (;;) {
+		wait_event_interruptible(srv->trb_waitq,
+					 !mtk_ctrl_chs_is_busy_or_empty(srv) ||
+					 kthread_should_stop() || kthread_should_park());
+		if (kthread_should_stop())
+			break;
+
+		if (kthread_should_park())
+			kthread_parkme();
+
+		do {
+			mtk_ctrl_trb_process(srv);
+			cond_resched();
+		} while (!mtk_ctrl_chs_is_busy_or_empty(srv) && !kthread_should_stop() &&
+			 !kthread_should_park());
+	}
+	mtk_ctrl_chs_flush(srv);
+	return 0;
+}
+
+static int mtk_ctrl_trb_srv_init(struct mtk_ctrl_trans *trans)
+{
+	struct srv_que *srv_que;
+	struct trb_srv *srv;
+	int i, j;
+	int ret;
+
+	for (i = 0; i < trans->trb_srv_num; i++) {
+		srv = kzalloc_obj(*srv);
+		if (!srv) {
+			ret = -ENOMEM;
+			goto err_free_srv;
+		}
+
+		srv->trans = trans;
+		srv->srv_id = i;
+		trans->trb_srv[i] = srv;
+
+		init_waitqueue_head(&srv->trb_waitq);
+		for (j = 0; j < NR_CLDMA; j++)
+			INIT_LIST_HEAD(&srv->srv_q_list[j]);
+	}
+
+	for (i = 0; i < NR_CLDMA; i++)
+		for (j = 0; j < HW_QUE_NUM; j++) {
+			if (trans->srv_cfg[i][j] < 0 ||
+			    trans->srv_cfg[i][j] >= trans->trb_srv_num)
+				trans->srv_cfg[i][j] = 0;
+			srv_que = kzalloc_obj(*srv_que);
+			if (!srv_que) {
+				ret = -ENOMEM;
+				goto err_free_srv_que;
+			}
+			srv_que->hif_id = i;
+			srv_que->qno = j;
+			list_add_tail(&srv_que->list,
+				      &trans->trb_srv[trans->srv_cfg[i][j]]->srv_q_list[i]);
+		}
+
+	for (i = 0; i < trans->trb_srv_num; i++) {
+		trans->trb_srv[i]->trb_thread = kthread_run(mtk_ctrl_trb_thread, trans->trb_srv[i],
+							    "mtk_trb_srv%d_%s", i,
+							    trans->mdev->dev_str);
+		if (IS_ERR(trans->trb_srv[i]->trb_thread)) {
+			ret = PTR_ERR(trans->trb_srv[i]->trb_thread);
+			trans->trb_srv[i]->trb_thread = NULL;
+			goto err_stop_kthread;
+		}
+	}
+
+	return 0;
+err_stop_kthread:
+	while (--i >= 0)
+		kthread_stop(trans->trb_srv[i]->trb_thread);
+err_free_srv_que:
+	for (i = 0; i < trans->trb_srv_num; i++) {
+		for (j = 0; j < NR_CLDMA; j++) {
+			struct srv_que *next_srv_que;
+
+			list_for_each_entry_safe(srv_que, next_srv_que,
+						 &trans->trb_srv[i]->srv_q_list[j], list) {
+				list_del(&srv_que->list);
+				kfree(srv_que);
+			}
+		}
+	}
+err_free_srv:
+	for (i = 0; i < trans->trb_srv_num; i++) {
+		if (!trans->trb_srv[i])
+			break;
+		kfree(trans->trb_srv[i]);
+		trans->trb_srv[i] = NULL;
+	}
+
+	return ret;
+}
+
+static void mtk_ctrl_trb_srv_exit(struct mtk_ctrl_trans *trans)
+{
+	struct srv_que *srv_que, *next_srv_que;
+	struct trb_srv *srv;
+	int i, j;
+
+	for (i = 0; i < trans->trb_srv_num; i++) {
+		srv = trans->trb_srv[i];
+		if (!srv)
+			continue;
+		kthread_stop(srv->trb_thread);
+		for (j = 0; j < NR_CLDMA; j++) {
+			list_for_each_entry_safe(srv_que, next_srv_que,
+						 &trans->trb_srv[i]->srv_q_list[j], list) {
+				list_del(&srv_que->list);
+				kfree(srv_que);
+			}
+		}
+		kfree(srv);
+		trans->trb_srv[i] = NULL;
+	}
+}
+
+static void mtk_ctrl_remove_radix_tree(struct mtk_ctrl_trans *trans)
+{
+	struct radix_tree_iter iter;
+	struct queue_info *queue;
+	void __rcu **slot;
+
+	radix_tree_for_each_slot(slot, &trans->queue_tbl, &iter, 0) {
+		queue = radix_tree_deref_slot(slot);
+		if (!queue)
+			continue;
+		radix_tree_delete(&trans->queue_tbl, iter.index);
+		kfree(queue);
+	}
+}
+
+int mtk_pcie_hif_init(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+	struct queue_info *queue, *queue_info;
+	struct mtk_ctrl_trans *trans;
+	int i, j;
+	int ret;
+
+	trans = ctrl_blk->ctrl_hw_priv;
+	trans->ctrl_blk = ctrl_blk;
+	queue_info = trans->queue_info;
+
+	INIT_RADIX_TREE(&trans->queue_tbl, GFP_KERNEL);
+	for (i = 0; i < trans->queue_info_num; i++) {
+		queue = kmemdup(queue_info + i, sizeof(*queue), GFP_KERNEL);
+		if (!queue) {
+			ret = -ENOMEM;
+			goto err_free_radix_tree;
+		}
+		if (queue->txqno >= HW_QUE_NUM || queue->rxqno >= HW_QUE_NUM ||
+		    queue->hif_id >= NR_CLDMA) {
+			dev_err(mdev->dev, "Failed to get correct queue info %x\n",
+				queue->rx_chl);
+			kfree(queue);
+			ret = -EINVAL;
+			goto err_free_radix_tree;
+		}
+		ret = radix_tree_insert(&trans->queue_tbl, queue->rx_chl & QUEUE_CHL_MASK, queue);
+		if (ret) {
+			dev_err(mdev->dev, "Insert %x fail, ret: %d", queue->rx_chl, ret);
+			kfree(queue);
+			goto err_free_radix_tree;
+		}
+	}
+
+	for (i = 0; i < NR_CLDMA; i++) {
+		for (j = 0; j < HW_QUE_NUM; j++) {
+			skb_queue_head_init(&trans->trans_list[i].skb_list[j]);
+			trans->trans_list[i].tx_burst_cnt[j] = 0;
+			/* usr_cnt tracks the queues rebuilt by mtk_cldma_init()
+			 * below, so it must be reset with them. Otherwise a
+			 * count left over from a torn-down cycle makes the
+			 * channel permanently unopenable.
+			 */
+			trans->usr_cnt[i][j] = 0;
+		}
+	}
+	ret = mtk_cldma_init(trans);
+	if (ret)
+		goto err_free_radix_tree;
+
+	ret = mtk_ctrl_trb_srv_init(trans);
+	if (ret)
+		goto err_cldma_exit;
+
+	atomic_set(&trans->available, 1);
+
+	return 0;
+
+err_cldma_exit:
+	mtk_cldma_exit(trans);
+err_free_radix_tree:
+	mtk_ctrl_remove_radix_tree(trans);
+
+	return ret;
+}
+
+int mtk_pcie_hif_exit(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+	struct mtk_ctrl_trans *trans;
+
+	trans = ctrl_blk->ctrl_hw_priv;
+
+	mutex_lock(&trans->submit_lock);
+	atomic_set(&trans->available, 0);
+	mutex_unlock(&trans->submit_lock);
+
+	/* Join the service threads before freeing what they dereference: they
+	 * read trans->dev without a lock, so clearing it cannot stop a
+	 * consumer that has already loaded the pointer.
+	 */
+	mtk_ctrl_trb_srv_exit(trans);
+	mtk_cldma_exit(trans);
+
+	/* Late submitters may still hold the lock and walk the tree. */
+	mutex_lock(&trans->submit_lock);
+	mtk_ctrl_remove_radix_tree(trans);
+	mutex_unlock(&trans->submit_lock);
+
+	return 0;
+}
+
+int mtk_pcie_hif_submit_skb(struct mtk_md_dev *mdev, struct sk_buff *skb, bool force_send)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+	struct mtk_ctrl_trans *trans;
+	struct queue_info *que;
+	struct trb *trb;
+	int ret;
+
+	trans = ctrl_blk->ctrl_hw_priv;
+	trb = (struct trb *)skb->cb;
+
+	if (trb->cmd == TRB_CMD_STOP || trb->cmd == TRB_CMD_RECOVER) {
+		trb->trb_complete(skb);
+		return 0;
+	}
+
+	mutex_lock(&trans->submit_lock);
+
+	if (!atomic_read(&trans->available)) {
+		ret = -EIO;
+		goto unlock;
+	}
+
+	que = radix_tree_lookup(&trans->queue_tbl, trb->channel_id & QUEUE_CHL_MASK);
+	if (!que) {
+		dev_warn(mdev->dev, "lookup que fail, ch_id: %x\n",
+			 trb->channel_id);
+		ret = -EINVAL;
+		goto unlock;
+	}
+
+	if (mtk_queue_list_is_full(trans, que) && !force_send) {
+		ret = -EAGAIN;
+		goto unlock;
+	}
+
+	if (trb->cmd == TRB_CMD_DISABLE) {
+		struct sk_buff *entry = NULL;
+		struct sk_buff_head *list;
+		struct sk_buff *iter;
+		unsigned long flags;
+
+		/* A disable may overtake queued data, so teardown does not
+		 * wait for a TX backlog, but it must never overtake a pending
+		 * ENABLE for the same queue: the two do not commute, and a
+		 * DISABLE consumed before its ENABLE closes nothing while the
+		 * ENABLE then arms rings nobody owns.
+		 */
+		list = &trans->trans_list[que->hif_id].skb_list[que->txqno];
+		spin_lock_irqsave(&list->lock, flags);
+		skb_queue_walk(list, iter) {
+			if (((struct trb *)iter->cb)->cmd != TRB_CMD_ENABLE) {
+				entry = iter;
+				break;
+			}
+		}
+		if (entry)
+			__skb_queue_before(list, entry, skb);
+		else
+			__skb_queue_tail(list, skb);
+		spin_unlock_irqrestore(&list->lock, flags);
+	} else {
+		skb_queue_tail(&trans->trans_list[que->hif_id].skb_list[que->txqno], skb);
+	}
+
+	wake_up(&trans->trb_srv[trans->srv_cfg[que->hif_id][que->txqno]]->trb_waitq);
+	ret = 0;
+
+unlock:
+	mutex_unlock(&trans->submit_lock);
+	return ret;
+}
+
+int mtk_pcie_hif_cmd_func(struct mtk_md_dev *mdev, int cmd, void *data)
+{
+	struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+	struct mtk_ctrl_trans *trans;
+	struct queue_info *que;
+	int ret;
+
+	switch (cmd) {
+	case HIF_CTRL_CMD_CHECK_TX_FULL:
+		trans = ctrl_blk->ctrl_hw_priv;
+		mutex_lock(&trans->submit_lock);
+		if (!atomic_read(&trans->available)) {
+			ret = -EIO;
+			break;
+		}
+		que = radix_tree_lookup(&trans->queue_tbl,
+					((union ctrl_hif_cmd_data *)data)->rx_ch & QUEUE_CHL_MASK);
+		if (!que) {
+			dev_warn(mdev->dev, "Failed to find que to check tx full\n");
+			ret = -EINVAL;
+			break;
+		}
+		ret = mtk_queue_list_is_full(trans, que);
+		break;
+	default:
+		return -EINVAL;
+	}
+	mutex_unlock(&trans->submit_lock);
+
+	return ret;
+}
+
+int mtk_trans_ctrl_init(struct mtk_md_dev *mdev)
+{
+	struct mtk_ctrl_trans *trans;
+	struct mtk_ctrl_blk *ctrl_blk;
+	int err;
+
+	trans = devm_kzalloc(mdev->dev, sizeof(*trans), GFP_KERNEL);
+	if (!trans)
+		return -ENOMEM;
+	trans->mdev = mdev;
+	mutex_init(&trans->submit_lock);
+	atomic_set(&trans->available, 0);
+
+	memcpy(trans->srv_cfg, mtk_srv_cfg, sizeof(mtk_srv_cfg));
+	trans->queue_info = (struct queue_info *)mtk_queue_info;
+	trans->queue_info_num = ARRAY_SIZE(mtk_queue_info);
+	trans->trb_srv_num = TRB_SRV_NUM;
+
+	err = mtk_ctrl_init(mdev);
+	if (err)
+		return err;
+
+	ctrl_blk = mdev->ctrl_blk;
+	ctrl_blk->ctrl_hw_priv = trans;
+
+	return 0;
+}
+
+int mtk_trans_ctrl_exit(struct mtk_md_dev *mdev)
+{
+	mtk_ctrl_exit(mdev);
+
+	return 0;
+}