The control port consists of port I/O and port manager.
Port I/O provides a common operation as defined by "struct port_ops",
and the operation is managed by the "port manager". It provides
interfaces to internal users, the implemented internal interfaces are
open, close, write and recv_register.
The port manager defines and implements port management interfaces and
structures. It is responsible for port creation, destroying, and managing
port states. It sends data from port I/O to CLDMA via TRB ( Transaction
Request Block ), and dispatches received data from CLDMA to port I/O.
The using port will be held in the "stale list" when the driver destroys
it, and after creating it again, the user can continue to use it.
Signed-off-by: Yanchao Yang <yanchao.yang@mediatek.com>
Signed-off-by: Felix Chen <felix.chen@mediatek.com>
---
drivers/net/wwan/mediatek/Makefile | 4 +-
drivers/net/wwan/mediatek/mtk_ctrl_plane.c | 117 +++
drivers/net/wwan/mediatek/mtk_ctrl_plane.h | 28 +-
drivers/net/wwan/mediatek/mtk_dev.c | 4 +-
drivers/net/wwan/mediatek/mtk_port.c | 844 +++++++++++++++++++++
drivers/net/wwan/mediatek/mtk_port.h | 171 +++++
drivers/net/wwan/mediatek/mtk_port_io.c | 253 ++++++
drivers/net/wwan/mediatek/mtk_port_io.h | 34 +
drivers/net/wwan/mediatek/pcie/mtk_pci.c | 2 +
9 files changed, 1452 insertions(+), 5 deletions(-)
create mode 100644 drivers/net/wwan/mediatek/mtk_port.c
create mode 100644 drivers/net/wwan/mediatek/mtk_port.h
create mode 100644 drivers/net/wwan/mediatek/mtk_port_io.c
create mode 100644 drivers/net/wwan/mediatek/mtk_port_io.h
diff --git a/drivers/net/wwan/mediatek/Makefile b/drivers/net/wwan/mediatek/Makefile
index 23a1cbc06ef6..e3afd8ecb494 100644
--- a/drivers/net/wwan/mediatek/Makefile
+++ b/drivers/net/wwan/mediatek/Makefile
@@ -7,6 +7,8 @@ mtk_tmi-y = \
mtk_dev.o \
mtk_ctrl_plane.o \
mtk_cldma.o \
- pcie/mtk_cldma_drv_t800.o
+ pcie/mtk_cldma_drv_t800.o \
+ mtk_port.o \
+ mtk_port_io.o
obj-$(CONFIG_MTK_TMI) += mtk_tmi.o
diff --git a/drivers/net/wwan/mediatek/mtk_ctrl_plane.c b/drivers/net/wwan/mediatek/mtk_ctrl_plane.c
index 1dbfcf8587a1..8adb1f53ec64 100644
--- a/drivers/net/wwan/mediatek/mtk_ctrl_plane.c
+++ b/drivers/net/wwan/mediatek/mtk_ctrl_plane.c
@@ -10,11 +10,118 @@
#include <linux/sched.h>
#include <linux/wait.h>
+#include "mtk_cldma.h"
#include "mtk_ctrl_plane.h"
+#include "mtk_port.h"
+
+static int mtk_ctrl_get_hif_id(unsigned char peer_id)
+{
+ if (peer_id == MTK_PEER_ID_SAP)
+ return CLDMA0;
+ else if (peer_id == MTK_PEER_ID_MD)
+ return CLDMA1;
+ else
+ return -EINVAL;
+}
+
+int mtk_ctrl_vq_search(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id,
+ unsigned char tx_hwq, unsigned char rx_hwq)
+{
+ struct mtk_port_mngr *port_mngr = ctrl_blk->port_mngr;
+ struct mtk_ctrl_trans *trans = ctrl_blk->trans;
+ int hif_id = mtk_ctrl_get_hif_id(peer_id);
+ struct virtq *vq;
+ int vq_num = 0;
+
+ if (hif_id < 0)
+ return -EINVAL;
+
+ do {
+ vq = trans->vq_tbl + vq_num;
+ if (port_mngr->vq_info[vq_num].color && vq->txqno == tx_hwq &&
+ vq->rxqno == rx_hwq && vq->hif_id == hif_id)
+ return vq_num;
+
+ vq_num++;
+ } while (vq_num < VQ_NUM);
+
+ return -ENOENT;
+}
+
+int mtk_ctrl_vq_color_paint(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id,
+ unsigned char tx_hwq, unsigned char rx_hwq,
+ unsigned int tx_mtu, unsigned int rx_mtu)
+{
+ struct mtk_port_mngr *port_mngr = ctrl_blk->port_mngr;
+ struct mtk_ctrl_trans *trans = ctrl_blk->trans;
+ int hif_id = mtk_ctrl_get_hif_id(peer_id);
+ struct virtq *vq;
+ int vq_num = 0;
+
+ if (hif_id < 0)
+ return -EINVAL;
+
+ do {
+ vq = trans->vq_tbl + vq_num;
+ if (vq->hif_id == hif_id && vq->txqno == tx_hwq && vq->rxqno == rx_hwq &&
+ vq->tx_mtu <= tx_mtu && vq->rx_mtu >= rx_mtu)
+ port_mngr->vq_info[vq_num].color = true;
+
+ vq_num++;
+ } while (vq_num < VQ_NUM);
+
+ return 0;
+}
+
+int mtk_ctrl_vq_color_cleanup(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id)
+{
+ struct mtk_port_mngr *port_mngr = ctrl_blk->port_mngr;
+ struct mtk_ctrl_trans *trans = ctrl_blk->trans;
+ int hif_id = mtk_ctrl_get_hif_id(peer_id);
+ struct virtq *vq;
+ int vq_num = 0;
+
+ if (hif_id < 0)
+ return -EINVAL;
+
+ do {
+ vq = trans->vq_tbl + vq_num;
+ if (vq->hif_id == hif_id)
+ port_mngr->vq_info[vq_num].color = false;
+
+ vq_num++;
+ } while (vq_num < VQ_NUM);
+
+ return 0;
+}
+
+int mtk_ctrl_trb_submit(struct mtk_ctrl_blk *blk, struct sk_buff *skb)
+{
+ struct mtk_ctrl_trans *trans = blk->trans;
+ struct trb *trb;
+ int vqno;
+
+ trb = (struct trb *)skb->cb;
+ if (trb->vqno >= VQ_NUM)
+ return -EINVAL;
+
+ if (!atomic_read(&trans->available))
+ return -EIO;
+
+ vqno = trb->vqno;
+ if (VQ_LIST_FULL(trans, vqno) && trb->cmd != TRB_CMD_DISABLE)
+ return -EAGAIN;
+
+ /* This function will implement in next patch */
+ wake_up(&trans->trb_srv->trb_waitq);
+
+ return 0;
+}
int mtk_ctrl_init(struct mtk_md_dev *mdev)
{
struct mtk_ctrl_blk *ctrl_blk;
+ int err;
ctrl_blk = devm_kzalloc(mdev->dev, sizeof(*ctrl_blk), GFP_KERNEL);
if (!ctrl_blk)
@@ -23,13 +130,23 @@ int mtk_ctrl_init(struct mtk_md_dev *mdev)
ctrl_blk->mdev = mdev;
mdev->ctrl_blk = ctrl_blk;
+ err = mtk_port_mngr_init(ctrl_blk);
+ if (err)
+ goto err_free_mem;
+
return 0;
+
+err_free_mem:
+ devm_kfree(mdev->dev, ctrl_blk);
+
+ return err;
}
int mtk_ctrl_exit(struct mtk_md_dev *mdev)
{
struct mtk_ctrl_blk *ctrl_blk = mdev->ctrl_blk;
+ mtk_port_mngr_exit(ctrl_blk);
devm_kfree(mdev->dev, ctrl_blk);
return 0;
diff --git a/drivers/net/wwan/mediatek/mtk_ctrl_plane.h b/drivers/net/wwan/mediatek/mtk_ctrl_plane.h
index 32cd8dc7bdb7..2e1f21d43644 100644
--- a/drivers/net/wwan/mediatek/mtk_ctrl_plane.h
+++ b/drivers/net/wwan/mediatek/mtk_ctrl_plane.h
@@ -11,13 +11,20 @@
#include "mtk_dev.h"
+#define VQ(N) (N)
+#define VQ_NUM (2)
+
#define VQ_MTU_3_5K (0xE00)
#define VQ_MTU_63K (0xFC00)
+#define SKB_LIST_MAX_LEN (16)
+
#define HIF_CLASS_NUM (1)
#define HIF_CLASS_SHIFT (8)
#define HIF_ID_BITMASK (0x01)
+#define VQ_LIST_FULL(trans, vqno) ((trans)->skb_list[vqno].qlen >= SKB_LIST_MAX_LEN)
+
enum mtk_trb_cmd_type {
TRB_CMD_ENABLE = 1,
TRB_CMD_TX,
@@ -39,6 +46,14 @@ struct trb {
int (*trb_complete)(struct sk_buff *skb);
};
+struct trb_srv {
+ int vq_cnt;
+ int vq_start;
+ struct mtk_ctrl_trans *trans;
+ wait_queue_head_t trb_waitq;
+ struct task_struct *trb_thread;
+};
+
struct virtq {
int vqno;
int hif_id;
@@ -50,8 +65,6 @@ struct virtq {
int rx_req_num;
};
-struct mtk_ctrl_trans;
-
struct hif_ops {
int (*init)(struct mtk_ctrl_trans *trans);
int (*exit)(struct mtk_ctrl_trans *trans);
@@ -60,18 +73,29 @@ struct hif_ops {
};
struct mtk_ctrl_trans {
+ struct sk_buff_head skb_list[VQ_NUM];
+ struct trb_srv *trb_srv;
struct virtq *vq_tbl;
void *dev[HIF_CLASS_NUM];
struct hif_ops *ops[HIF_CLASS_NUM];
struct mtk_ctrl_blk *ctrl_blk;
struct mtk_md_dev *mdev;
+ atomic_t available;
};
struct mtk_ctrl_blk {
struct mtk_md_dev *mdev;
+ struct mtk_port_mngr *port_mngr;
struct mtk_ctrl_trans *trans;
};
+int mtk_ctrl_vq_search(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id,
+ unsigned char tx_hwq, unsigned char rx_hwq);
+int mtk_ctrl_vq_color_paint(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id,
+ unsigned char tx_hwq, unsigned char rx_hwq,
+ unsigned int tx_mtu, unsigned int rx_mtu);
+int mtk_ctrl_vq_color_cleanup(struct mtk_ctrl_blk *ctrl_blk, unsigned char peer_id);
+int mtk_ctrl_trb_submit(struct mtk_ctrl_blk *blk, struct sk_buff *skb);
int mtk_ctrl_init(struct mtk_md_dev *mdev);
int mtk_ctrl_exit(struct mtk_md_dev *mdev);
diff --git a/drivers/net/wwan/mediatek/mtk_dev.c b/drivers/net/wwan/mediatek/mtk_dev.c
index f63c7e04df6a..d34c3933e84d 100644
--- a/drivers/net/wwan/mediatek/mtk_dev.c
+++ b/drivers/net/wwan/mediatek/mtk_dev.c
@@ -12,10 +12,10 @@ int mtk_dev_init(struct mtk_md_dev *mdev)
ret = mtk_ctrl_init(mdev);
if (ret)
- goto err_ctrl_init;
+ goto exit;
return 0;
-err_ctrl_init:
+exit:
return ret;
}
diff --git a/drivers/net/wwan/mediatek/mtk_port.c b/drivers/net/wwan/mediatek/mtk_port.c
new file mode 100644
index 000000000000..12097a279aa0
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_port.c
@@ -0,0 +1,844 @@
+// SPDX-License-Identifier: BSD-3-Clause-Clear
+/*
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#include <linux/bitfield.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+
+#include "mtk_port.h"
+#include "mtk_port_io.h"
+
+#define MTK_DFLT_TRB_TIMEOUT (5 * HZ)
+#define MTK_DFLT_TRB_STATUS (0x1)
+#define MTK_CHECK_RX_SEQ_MASK (0x7fff)
+
+#define MTK_PORT_SEARCH_FROM_RADIX_TREE(p, s) ({\
+ struct mtk_port *_p; \
+ _p = radix_tree_deref_slot(s); \
+ if (!_p) \
+ continue; \
+ p = _p; \
+})
+
+#define MTK_PORT_INTERNAL_NODE_CHECK(p, s, i) ({\
+ if (radix_tree_is_internal_node(p)) { \
+ s = radix_tree_iter_retry(&(i));\
+ continue; \
+ } \
+})
+
+static LIST_HEAD(stale_list_grp);
+/* mutex lock for stale_list_group */
+DEFINE_MUTEX(port_mngr_grp_mtx);
+
+static DEFINE_IDA(ccci_dev_ids);
+
+static const struct mtk_port_cfg port_cfg[] = {
+ {CCCI_CONTROL_TX, CCCI_CONTROL_RX, VQ(1), PORT_TYPE_INTERNAL, "MDCTRL", PORT_F_ALLOW_DROP},
+ {CCCI_SAP_CONTROL_TX, CCCI_SAP_CONTROL_RX, VQ(0), PORT_TYPE_INTERNAL, "SAPCTRL",
+ PORT_F_ALLOW_DROP},
+};
+
+void mtk_port_release(struct kref *port_kref)
+{
+ struct mtk_stale_list *s_list;
+ struct mtk_port *port;
+
+ port = container_of(port_kref, struct mtk_port, kref);
+ if (!test_bit(PORT_S_ON_STALE_LIST, &port->status))
+ goto port_exit;
+
+ list_del(&port->stale_entry);
+ list_for_each_entry(s_list, &stale_list_grp, entry) {
+ if (!strncmp(s_list->dev_str, port->dev_str, MTK_DEV_STR_LEN) &&
+ list_empty(&s_list->ports) && s_list->dev_id >= 0) {
+ pr_info("Free dev id of stale list(%s)\n", s_list->dev_str);
+ ida_free(&ccci_dev_ids, s_list->dev_id);
+ s_list->dev_id = -1;
+ break;
+ }
+ }
+
+port_exit:
+ ports_ops[port->info.type]->exit(port);
+ kfree(port);
+}
+
+static int mtk_port_tbl_add(struct mtk_port_mngr *port_mngr, struct mtk_port *port)
+{
+ int ret;
+
+ ret = radix_tree_insert(&port_mngr->port_tbl[MTK_PORT_TBL_TYPE(port->info.rx_ch)],
+ port->info.rx_ch & 0xFFF, port);
+ if (ret)
+ dev_err(port_mngr->ctrl_blk->mdev->dev,
+ "port(%s) add to port_tbl failed, return %d\n",
+ port->info.name, ret);
+
+ return ret;
+}
+
+static void mtk_port_tbl_del(struct mtk_port_mngr *port_mngr, struct mtk_port *port)
+{
+ radix_tree_delete(&port_mngr->port_tbl[MTK_PORT_TBL_TYPE(port->info.rx_ch)],
+ port->info.rx_ch & 0xFFF);
+}
+
+static struct mtk_port *mtk_port_get_from_stale_list(struct mtk_port_mngr *port_mngr,
+ struct mtk_stale_list *s_list,
+ int rx_ch)
+{
+ struct mtk_port *port, *next_port;
+ int ret;
+
+ mutex_lock(&port_mngr_grp_mtx);
+ list_for_each_entry_safe(port, next_port, &s_list->ports, stale_entry) {
+ if (port->info.rx_ch == rx_ch) {
+ kref_get(&port->kref);
+ list_del(&port->stale_entry);
+ ret = mtk_port_tbl_add(port_mngr, port);
+ if (ret) {
+ list_add_tail(&port->stale_entry, &s_list->ports);
+ kref_put(&port->kref, mtk_port_release);
+ mutex_unlock(&port_mngr_grp_mtx);
+ dev_err(port_mngr->ctrl_blk->mdev->dev,
+ "Failed when adding (%s) to port mngr\n",
+ port->info.name);
+ return ERR_PTR(ret);
+ }
+
+ port->port_mngr = port_mngr;
+ clear_bit(PORT_S_ON_STALE_LIST, &port->status);
+ mutex_unlock(&port_mngr_grp_mtx);
+ return port;
+ }
+ }
+ mutex_unlock(&port_mngr_grp_mtx);
+
+ return NULL;
+}
+
+static struct mtk_port *mtk_port_alloc_or_restore(struct mtk_port_mngr *port_mngr,
+ struct mtk_port_cfg *dflt_info,
+ struct mtk_stale_list *s_list)
+{
+ struct mtk_port *port;
+ int ret;
+
+ port = mtk_port_get_from_stale_list(port_mngr, s_list, dflt_info->rx_ch);
+ if (IS_ERR(port))
+ return port;
+
+ if (port) {
+ ports_ops[port->info.type]->reset(port);
+ dev_info(port_mngr->ctrl_blk->mdev->dev,
+ "Port(%s) move from stale list\n", port->info.name);
+ goto return_port;
+ }
+
+ port = kzalloc(sizeof(*port), GFP_KERNEL);
+ if (!port) {
+ ret = -ENOMEM;
+ goto exit_err;
+ }
+
+ memcpy(port, dflt_info, sizeof(*dflt_info));
+ ret = mtk_port_tbl_add(port_mngr, port);
+ if (ret < 0) {
+ dev_err(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to add port(%s) to port tbl\n", dflt_info->name);
+ goto free_port;
+ }
+
+ port->port_mngr = port_mngr;
+ ports_ops[port->info.type]->init(port);
+ dev_info(port_mngr->ctrl_blk->mdev->dev,
+ "Port(%s) alloc and init\n", port->info.name);
+
+return_port:
+ return port;
+free_port:
+ kfree(port);
+exit_err:
+ return ERR_PTR(ret);
+}
+
+static void mtk_port_free_or_backup(struct mtk_port_mngr *port_mngr,
+ struct mtk_port *port, struct mtk_stale_list *s_list)
+{
+ mutex_lock(&port_mngr_grp_mtx);
+ mtk_port_tbl_del(port_mngr, port);
+ if (port->info.type != PORT_TYPE_INTERNAL) {
+ if (test_bit(PORT_S_OPEN, &port->status)) {
+ list_add_tail(&port->stale_entry, &s_list->ports);
+ set_bit(PORT_S_ON_STALE_LIST, &port->status);
+ dev_info(port_mngr->ctrl_blk->mdev->dev,
+ "Port(%s) move to stale list\n", port->info.name);
+ memcpy(port->dev_str, port_mngr->ctrl_blk->mdev->dev_str, MTK_DEV_STR_LEN);
+ port->port_mngr = NULL;
+ }
+ kref_put(&port->kref, mtk_port_release);
+ } else {
+ mtk_port_release(&port->kref);
+ }
+ mutex_unlock(&port_mngr_grp_mtx);
+}
+
+static struct mtk_port *mtk_port_search_by_id(struct mtk_port_mngr *port_mngr, int rx_ch)
+{
+ int tbl_type = MTK_PORT_TBL_TYPE(rx_ch);
+
+ if (tbl_type < PORT_TBL_SAP || tbl_type >= PORT_TBL_MAX)
+ return NULL;
+
+ return radix_tree_lookup(&port_mngr->port_tbl[tbl_type], MTK_CH_ID(rx_ch));
+}
+
+struct mtk_port *mtk_port_search_by_name(struct mtk_port_mngr *port_mngr, char *name)
+{
+ int tbl_type = PORT_TBL_SAP;
+ struct radix_tree_iter iter;
+ struct mtk_port *port;
+ void __rcu **slot;
+
+ do {
+ radix_tree_for_each_slot(slot, &port_mngr->port_tbl[tbl_type], &iter, 0) {
+ MTK_PORT_SEARCH_FROM_RADIX_TREE(port, slot);
+ MTK_PORT_INTERNAL_NODE_CHECK(port, slot, iter);
+ if (!strncmp(port->info.name, name, strlen(port->info.name)))
+ return port;
+ }
+ tbl_type++;
+ } while (tbl_type < PORT_TBL_MAX);
+ return NULL;
+}
+
+static int mtk_port_tbl_create(struct mtk_port_mngr *port_mngr, struct mtk_port_cfg *cfg,
+ const int port_cnt, struct mtk_stale_list *s_list)
+{
+ struct mtk_port_cfg *dflt_port;
+ struct mtk_port *port;
+ int i, ret;
+
+ INIT_RADIX_TREE(&port_mngr->port_tbl[PORT_TBL_SAP], GFP_KERNEL);
+ INIT_RADIX_TREE(&port_mngr->port_tbl[PORT_TBL_MD], GFP_KERNEL);
+
+ for (i = 0; i < port_cnt; i++) {
+ dflt_port = cfg + i;
+ port = mtk_port_alloc_or_restore(port_mngr, dflt_port, s_list);
+ if (IS_ERR(port)) {
+ ret = PTR_ERR(port);
+ goto free_ports;
+ }
+ }
+ return 0;
+
+free_ports:
+ for (i--; i >= 0; i--) {
+ dflt_port = cfg + i;
+ port = mtk_port_search_by_id(port_mngr, dflt_port->rx_ch);
+ if (port)
+ mtk_port_free_or_backup(port_mngr, port, s_list);
+ }
+
+ return ret;
+}
+
+static void mtk_port_tbl_destroy(struct mtk_port_mngr *port_mngr, struct mtk_stale_list *s_list)
+{
+ struct radix_tree_iter iter;
+ struct mtk_port *port;
+ void __rcu **slot;
+ int tbl_type;
+
+ tbl_type = PORT_TBL_SAP;
+ do {
+ radix_tree_for_each_slot(slot, &port_mngr->port_tbl[tbl_type], &iter, 0) {
+ MTK_PORT_SEARCH_FROM_RADIX_TREE(port, slot);
+ MTK_PORT_INTERNAL_NODE_CHECK(port, slot, iter);
+ ports_ops[port->info.type]->disable(port);
+ }
+ tbl_type++;
+ } while (tbl_type < PORT_TBL_MAX);
+
+ tbl_type = PORT_TBL_SAP;
+ do {
+ radix_tree_for_each_slot(slot, &port_mngr->port_tbl[tbl_type], &iter, 0) {
+ MTK_PORT_SEARCH_FROM_RADIX_TREE(port, slot);
+ MTK_PORT_INTERNAL_NODE_CHECK(port, slot, iter);
+ mtk_port_free_or_backup(port_mngr, port, s_list);
+ }
+ tbl_type++;
+ } while (tbl_type < PORT_TBL_MAX);
+}
+
+static struct mtk_stale_list *mtk_port_stale_list_create(struct mtk_port_mngr *port_mngr)
+{
+ struct mtk_stale_list *s_list;
+
+ s_list = kzalloc(sizeof(*s_list), GFP_KERNEL);
+ if (!s_list)
+ return NULL;
+
+ memcpy(s_list->dev_str, port_mngr->ctrl_blk->mdev->dev_str, MTK_DEV_STR_LEN);
+ s_list->dev_id = -1;
+ INIT_LIST_HEAD(&s_list->ports);
+
+ mutex_lock(&port_mngr_grp_mtx);
+ list_add_tail(&s_list->entry, &stale_list_grp);
+ mutex_unlock(&port_mngr_grp_mtx);
+
+ return s_list;
+}
+
+static void mtk_port_stale_list_destroy(struct mtk_stale_list *s_list)
+{
+ mutex_lock(&port_mngr_grp_mtx);
+ list_del(&s_list->entry);
+ mutex_unlock(&port_mngr_grp_mtx);
+ kfree(s_list);
+}
+
+static struct mtk_stale_list *mtk_port_stale_list_search(const char *dev_str)
+{
+ struct mtk_stale_list *tmp, *s_list = NULL;
+
+ mutex_lock(&port_mngr_grp_mtx);
+ list_for_each_entry(tmp, &stale_list_grp, entry) {
+ if (!strncmp(tmp->dev_str, dev_str, MTK_DEV_STR_LEN)) {
+ s_list = tmp;
+ break;
+ }
+ }
+ mutex_unlock(&port_mngr_grp_mtx);
+
+ return s_list;
+}
+
+void mtk_port_stale_list_grp_cleanup(void)
+{
+ struct mtk_stale_list *s_list, *next_s_list;
+ struct mtk_port *port, *next_port;
+
+ mutex_lock(&port_mngr_grp_mtx);
+ list_for_each_entry_safe(s_list, next_s_list, &stale_list_grp, entry) {
+ list_del(&s_list->entry);
+
+ list_for_each_entry_safe(port, next_port, &s_list->ports, stale_entry) {
+ list_del(&port->stale_entry);
+ mtk_port_release(&port->kref);
+ }
+
+ kfree(s_list);
+ }
+ mutex_unlock(&port_mngr_grp_mtx);
+}
+
+static struct mtk_stale_list *mtk_port_stale_list_init(struct mtk_port_mngr *port_mngr)
+{
+ struct mtk_stale_list *s_list;
+
+ s_list = mtk_port_stale_list_search(port_mngr->ctrl_blk->mdev->dev_str);
+ if (!s_list) {
+ dev_info(port_mngr->ctrl_blk->mdev->dev, "Create stale list\n");
+ s_list = mtk_port_stale_list_create(port_mngr);
+ if (unlikely(!s_list))
+ return NULL;
+ } else {
+ dev_info(port_mngr->ctrl_blk->mdev->dev, "Reuse old stale list\n");
+ }
+
+ mutex_lock(&port_mngr_grp_mtx);
+ if (s_list->dev_id < 0) {
+ port_mngr->dev_id = ida_alloc_range(&ccci_dev_ids, 0,
+ MTK_DFLT_MAX_DEV_CNT - 1,
+ GFP_KERNEL);
+ } else {
+ port_mngr->dev_id = s_list->dev_id;
+ s_list->dev_id = -1;
+ }
+ mutex_unlock(&port_mngr_grp_mtx);
+
+ return s_list;
+}
+
+static void mtk_port_stale_list_exit(struct mtk_port_mngr *port_mngr, struct mtk_stale_list *s_list)
+{
+ mutex_lock(&port_mngr_grp_mtx);
+ if (list_empty(&s_list->ports)) {
+ ida_free(&ccci_dev_ids, port_mngr->dev_id);
+ mutex_unlock(&port_mngr_grp_mtx);
+ mtk_port_stale_list_destroy(s_list);
+ dev_info(port_mngr->ctrl_blk->mdev->dev, "Destroy stale list\n");
+ } else {
+ s_list->dev_id = port_mngr->dev_id;
+ mutex_unlock(&port_mngr_grp_mtx);
+ dev_info(port_mngr->ctrl_blk->mdev->dev, "Reserve stale list\n");
+ }
+}
+
+static void mtk_port_trb_init(struct mtk_port *port, struct trb *trb, enum mtk_trb_cmd_type cmd,
+ int (*trb_complete)(struct sk_buff *skb))
+{
+ kref_init(&trb->kref);
+ trb->vqno = port->info.vq_id;
+ trb->status = MTK_DFLT_TRB_STATUS;
+ trb->priv = port;
+ trb->cmd = cmd;
+ trb->trb_complete = trb_complete;
+}
+
+static void mtk_port_trb_free(struct kref *trb_kref)
+{
+ struct trb *trb = container_of(trb_kref, struct trb, kref);
+ struct sk_buff *skb;
+
+ skb = container_of((char *)trb, struct sk_buff, cb[0]);
+ dev_kfree_skb_any(skb);
+}
+
+static int mtk_port_open_trb_complete(struct sk_buff *skb)
+{
+ struct trb_open_priv *trb_open_priv = (struct trb_open_priv *)skb->data;
+ struct trb *trb = (struct trb *)skb->cb;
+ struct mtk_port *port = trb->priv;
+ struct mtk_port_mngr *port_mngr;
+
+ port_mngr = port->port_mngr;
+
+ if (trb->status && trb->status != -EBUSY)
+ goto out;
+
+ if (!trb->status) {
+ port_mngr->vq_info[trb->vqno].tx_mtu = trb_open_priv->tx_mtu;
+ port_mngr->vq_info[trb->vqno].rx_mtu = trb_open_priv->rx_mtu;
+ }
+
+ port->tx_mtu = port_mngr->vq_info[trb->vqno].tx_mtu;
+ port->rx_mtu = port_mngr->vq_info[trb->vqno].rx_mtu;
+
+ port->tx_mtu -= MTK_CCCI_H_ELEN;
+ port->rx_mtu -= MTK_CCCI_H_ELEN;
+
+out:
+ wake_up_interruptible_all(&port->trb_wq);
+
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Open VQ TRB:status:%d, vq:%d, port:%s, tx_mtu:%d. rx_mtu:%d\n",
+ trb->status, trb->vqno, port->info.name, port->tx_mtu, port->rx_mtu);
+ kref_put(&trb->kref, mtk_port_trb_free);
+ return 0;
+}
+
+static int mtk_port_close_trb_complete(struct sk_buff *skb)
+{
+ struct trb *trb = (struct trb *)skb->cb;
+ struct mtk_port *port = trb->priv;
+
+ wake_up_interruptible_all(&port->trb_wq);
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Close VQ TRB: trb->status:%d, vq:%d, port:%s\n",
+ trb->status, trb->vqno, port->info.name);
+ kref_put(&trb->kref, mtk_port_trb_free);
+
+ return 0;
+}
+
+static int mtk_port_tx_complete(struct sk_buff *skb)
+{
+ struct trb *trb = (struct trb *)skb->cb;
+ struct mtk_port *port = trb->priv;
+
+ if (trb->status < 0)
+ dev_warn(port->port_mngr->ctrl_blk->mdev->dev,
+ "Failed to send data: trb->status:%d, vq:%d, port:%s\n",
+ trb->status, trb->vqno, port->info.name);
+
+ if (port->info.flags & PORT_F_BLOCKING)
+ wake_up_interruptible_all(&port->trb_wq);
+
+ kref_put(&trb->kref, mtk_port_trb_free);
+
+ return 0;
+}
+
+static int mtk_port_status_check(struct mtk_port *port)
+{
+ if (!test_bit(PORT_S_ENABLE, &port->status)) {
+ pr_err("[TMI]Unable to use port: (%s) disabled. Caller: %ps\n",
+ port->info.name, __builtin_return_address(0));
+ return -ENODEV;
+ }
+
+ if (!test_bit(PORT_S_OPEN, &port->status) || test_bit(PORT_S_FLUSH, &port->status) ||
+ !test_bit(PORT_S_RDWR, &port->status)) {
+ dev_err(port->port_mngr->ctrl_blk->mdev->dev,
+ "Unable to use port: (%s), port status = 0x%lx. Caller: %ps\n",
+ port->info.name, port->status, __builtin_return_address(0));
+
+ return -EBADF;
+ }
+
+ return 0;
+}
+
+int mtk_port_send_data(struct mtk_port *port, void *data)
+{
+ struct mtk_port_mngr *port_mngr;
+ struct mtk_ctrl_trans *trans;
+ struct sk_buff *skb = data;
+ struct trb *trb;
+ int ret, len;
+
+ port_mngr = port->port_mngr;
+ trans = port_mngr->ctrl_blk->trans;
+
+ trb = (struct trb *)skb->cb;
+ mtk_port_trb_init(port, trb, TRB_CMD_TX, mtk_port_tx_complete);
+ len = skb->len;
+ kref_get(&trb->kref); /* kref count 1->2 */
+
+submit_trb:
+ mutex_lock(&port->write_lock);
+ ret = mtk_port_status_check(port);
+ if (!ret)
+ ret = mtk_ctrl_trb_submit(port_mngr->ctrl_blk, skb);
+ mutex_unlock(&port->write_lock);
+
+ if (ret == -EAGAIN && port->info.flags & PORT_F_BLOCKING) {
+ dev_warn(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to submit trb for port(%s), ret=%d\n", port->info.name, ret);
+ wait_event_interruptible(port->trb_wq, !VQ_LIST_FULL(trans, trb->vqno));
+ goto submit_trb;
+ } else if (ret < 0) {
+ dev_warn(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to submit trb for port(%s), ret=%d\n", port->info.name, ret);
+ kref_put(&trb->kref, mtk_port_trb_free); /* kref count 2->1 */
+ dev_kfree_skb_any(skb);
+ goto end;
+ }
+
+ if (!(port->info.flags & PORT_F_BLOCKING)) {
+ kref_put(&trb->kref, mtk_port_trb_free);
+ ret = len;
+ goto end;
+ }
+start_wait:
+ ret = wait_event_interruptible_timeout(port->trb_wq,
+ trb->status <= 0 ||
+ test_bit(PORT_S_FLUSH, &port->status),
+ MTK_DFLT_TRB_TIMEOUT);
+
+ if (ret == -ERESTARTSYS)
+ goto start_wait;
+ else if (test_bit(PORT_S_FLUSH, &port->status))
+ ret = -EBUSY;
+ else if (!ret)
+ ret = -ETIMEDOUT;
+ else
+ ret = (!trb->status) ? len : trb->status;
+
+ kref_put(&trb->kref, mtk_port_trb_free);
+
+end:
+ return ret;
+}
+
+static int mtk_port_check_rx_seq(struct mtk_port *port, struct mtk_ccci_header *ccci_h)
+{
+ u16 seq_num, assert_bit;
+
+ seq_num = FIELD_GET(MTK_HDR_FLD_SEQ, le32_to_cpu(ccci_h->status));
+ assert_bit = FIELD_GET(MTK_HDR_FLD_AST, le32_to_cpu(ccci_h->status));
+ if (assert_bit && port->rx_seq &&
+ ((seq_num - port->rx_seq) & MTK_CHECK_RX_SEQ_MASK) != 1) {
+ dev_err(port->port_mngr->ctrl_blk->mdev->dev,
+ "<ch: %ld> seq num out-of-order %d->%d",
+ FIELD_GET(MTK_HDR_FLD_CHN, le32_to_cpu(ccci_h->status)),
+ seq_num, port->rx_seq);
+ return -EPROTO;
+ }
+
+ return 0;
+}
+
+static int mtk_port_rx_dispatch(struct sk_buff *skb, int len, void *priv)
+{
+ struct mtk_port_mngr *port_mngr;
+ struct mtk_ccci_header *ccci_h;
+ struct mtk_port *port = priv;
+ int ret = -EPROTO;
+ u16 channel;
+
+ if (!skb || !priv) {
+ pr_err("[TMI] Invalid input value in rx dispatch\n");
+ ret = -EINVAL;
+ goto end;
+ }
+
+ port_mngr = port->port_mngr;
+
+ skb->len = 0;
+ skb_reset_tail_pointer(skb);
+ skb_put(skb, len);
+
+ ccci_h = mtk_port_strip_header(skb);
+ if (unlikely(!ccci_h)) {
+ dev_warn(port_mngr->ctrl_blk->mdev->dev,
+ "Unsupported: skb length(%d) is less than ccci header\n",
+ skb->len);
+ goto drop_data;
+ }
+
+ dev_dbg(port_mngr->ctrl_blk->mdev->dev,
+ "RX header:%08x %08x\n", ccci_h->packet_len, ccci_h->status);
+
+ channel = FIELD_GET(MTK_HDR_FLD_CHN, le32_to_cpu(ccci_h->status));
+ port = mtk_port_search_by_id(port_mngr, channel);
+ if (unlikely(!port)) {
+ dev_warn(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to find port by channel:%d\n", channel);
+ goto drop_data;
+ }
+
+ ret = mtk_port_check_rx_seq(port, ccci_h);
+ if (unlikely(ret))
+ goto drop_data;
+
+ port->rx_seq = FIELD_GET(MTK_HDR_FLD_SEQ, le32_to_cpu(ccci_h->status));
+
+ ret = ports_ops[port->info.type]->recv(port, skb);
+
+ return ret;
+
+drop_data:
+ dev_kfree_skb_any(skb);
+end:
+ return ret;
+}
+
+int mtk_port_add_header(struct sk_buff *skb)
+{
+ struct mtk_ccci_header *ccci_h;
+ struct mtk_port *port;
+ struct trb *trb;
+ int ret = 0;
+
+ trb = (struct trb *)skb->cb;
+ if (trb->status == 0xADDED)
+ goto end;
+
+ port = trb->priv;
+ if (!port) {
+ ret = -EINVAL;
+ goto end;
+ }
+
+ ccci_h = skb_push(skb, sizeof(*ccci_h));
+
+ ccci_h->packet_header = cpu_to_le32(0);
+ ccci_h->packet_len = cpu_to_le32(skb->len);
+ ccci_h->ex_msg = cpu_to_le32(0);
+ ccci_h->status = cpu_to_le32(FIELD_PREP(MTK_HDR_FLD_CHN, port->info.tx_ch) |
+ FIELD_PREP(MTK_HDR_FLD_SEQ, port->tx_seq++) |
+ FIELD_PREP(MTK_HDR_FLD_AST, 1));
+
+ trb->status = 0xADDED;
+end:
+ return ret;
+}
+
+struct mtk_ccci_header *mtk_port_strip_header(struct sk_buff *skb)
+{
+ struct mtk_ccci_header *ccci_h;
+
+ if (skb->len < sizeof(*ccci_h)) {
+ pr_err("[TMI] Invalid input value\n");
+ return NULL;
+ }
+
+ ccci_h = (struct mtk_ccci_header *)skb->data;
+ skb_pull(skb, sizeof(*ccci_h));
+
+ return ccci_h;
+}
+
+int mtk_port_mngr_vq_status_check(struct sk_buff *skb)
+{
+ struct trb *trb = (struct trb *)skb->cb;
+ struct trb_open_priv *trb_open_priv;
+ struct mtk_port *port = trb->priv;
+ struct mtk_port_mngr *port_mngr;
+ int ret = 0;
+
+ port_mngr = port->port_mngr;
+ switch (trb->cmd) {
+ case TRB_CMD_ENABLE:
+ port_mngr->vq_info[trb->vqno].port_cnt++;
+ if (port_mngr->vq_info[trb->vqno].port_cnt == 1) {
+ trb_open_priv = (struct trb_open_priv *)skb->data;
+ trb_open_priv->rx_done = mtk_port_rx_dispatch;
+ break;
+ }
+
+ trb->status = -EBUSY;
+ trb->trb_complete(skb);
+ ret = -EBUSY;
+ break;
+ case TRB_CMD_DISABLE:
+ port_mngr->vq_info[trb->vqno].port_cnt--;
+ if (!port_mngr->vq_info[trb->vqno].port_cnt)
+ break;
+
+ dev_info(port_mngr->ctrl_blk->mdev->dev,
+ "VQ(%d) still has %d port, skip to handle close skb\n",
+ trb->vqno, port_mngr->vq_info[trb->vqno].port_cnt);
+ trb->status = -EBUSY;
+ trb->trb_complete(skb);
+ ret = -EBUSY;
+ break;
+ default:
+ dev_err(port_mngr->ctrl_blk->mdev->dev, "Invalid trb command(%d)\n", trb->cmd);
+ ret = -EINVAL;
+ break;
+ }
+ return ret;
+}
+
+int mtk_port_vq_enable(struct mtk_port *port)
+{
+ struct mtk_port_mngr *port_mngr = port->port_mngr;
+ struct sk_buff *skb;
+ int ret = -ENOMEM;
+ struct trb *trb;
+
+ skb = __dev_alloc_skb(port->tx_mtu, GFP_KERNEL);
+ if (!skb)
+ goto end;
+
+ skb_put(skb, sizeof(struct trb_open_priv));
+ trb = (struct trb *)skb->cb;
+ mtk_port_trb_init(port, trb, TRB_CMD_ENABLE, mtk_port_open_trb_complete);
+ kref_get(&trb->kref);
+
+ ret = mtk_ctrl_trb_submit(port_mngr->ctrl_blk, skb);
+ if (ret) {
+ dev_err(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to submit trb for port(%s), ret=%d\n", port->info.name, ret);
+ kref_put(&trb->kref, mtk_port_trb_free);
+ mtk_port_trb_free(&trb->kref);
+ goto end;
+ }
+
+start_wait:
+ ret = wait_event_interruptible_timeout(port->trb_wq, trb->status <= 0,
+ MTK_DFLT_TRB_TIMEOUT);
+ if (ret == -ERESTARTSYS)
+ goto start_wait;
+ else if (!ret)
+ ret = -ETIMEDOUT;
+ else
+ ret = trb->status;
+
+ kref_put(&trb->kref, mtk_port_trb_free);
+
+end:
+ return ret;
+}
+
+int mtk_port_vq_disable(struct mtk_port *port)
+{
+ struct mtk_port_mngr *port_mngr = port->port_mngr;
+ struct sk_buff *skb;
+ int ret = -ENOMEM;
+ struct trb *trb;
+
+ skb = __dev_alloc_skb(port->tx_mtu, GFP_KERNEL);
+ if (!skb)
+ goto end;
+
+ skb_put(skb, sizeof(struct trb_open_priv));
+ trb = (struct trb *)skb->cb;
+ mtk_port_trb_init(port, trb, TRB_CMD_DISABLE, mtk_port_close_trb_complete);
+ kref_get(&trb->kref);
+
+ mutex_lock(&port->write_lock);
+ ret = mtk_ctrl_trb_submit(port_mngr->ctrl_blk, skb);
+ mutex_unlock(&port->write_lock);
+ if (ret) {
+ dev_warn(port_mngr->ctrl_blk->mdev->dev,
+ "Failed to submit trb for port(%s), ret=%d\n", port->info.name, ret);
+ kref_put(&trb->kref, mtk_port_trb_free);
+ mtk_port_trb_free(&trb->kref);
+ goto end;
+ }
+
+start_wait:
+ ret = wait_event_interruptible(port->trb_wq, trb->status <= 0);
+ if (ret == -ERESTARTSYS)
+ goto start_wait;
+
+ ret = trb->status;
+ kref_put(&trb->kref, mtk_port_trb_free);
+
+end:
+ return ret;
+}
+
+int mtk_port_mngr_init(struct mtk_ctrl_blk *ctrl_blk)
+{
+ struct mtk_port_mngr *port_mngr;
+ struct mtk_stale_list *s_list;
+ int ret = -ENOMEM;
+
+ port_mngr = devm_kzalloc(ctrl_blk->mdev->dev, sizeof(*port_mngr), GFP_KERNEL);
+ if (unlikely(!port_mngr)) {
+ dev_err(ctrl_blk->mdev->dev, "Failed to alloc memory for port_mngr\n");
+ goto end;
+ }
+
+ port_mngr->ctrl_blk = ctrl_blk;
+
+ s_list = mtk_port_stale_list_init(port_mngr);
+ if (!s_list) {
+ dev_err(ctrl_blk->mdev->dev, "Failed to init mtk_stale_list\n");
+ goto free_port_mngr;
+ }
+
+ ret = mtk_port_tbl_create(port_mngr, (struct mtk_port_cfg *)port_cfg,
+ ARRAY_SIZE(port_cfg), s_list);
+ if (unlikely(ret)) {
+ dev_err(ctrl_blk->mdev->dev, "Failed to create port_tbl\n");
+ goto exit_stale_list;
+ }
+ ctrl_blk->port_mngr = port_mngr;
+ dev_info(ctrl_blk->mdev->dev, "Initialize port_mngr successfully\n");
+
+ return ret;
+
+exit_stale_list:
+ mtk_port_stale_list_exit(port_mngr, s_list);
+free_port_mngr:
+ devm_kfree(ctrl_blk->mdev->dev, port_mngr);
+end:
+ return ret;
+}
+
+void mtk_port_mngr_exit(struct mtk_ctrl_blk *ctrl_blk)
+{
+ struct mtk_port_mngr *port_mngr = ctrl_blk->port_mngr;
+ struct mtk_stale_list *s_list;
+
+ s_list = mtk_port_stale_list_search(port_mngr->ctrl_blk->mdev->dev_str);
+ mtk_port_tbl_destroy(port_mngr, s_list);
+ mtk_port_stale_list_exit(port_mngr, s_list);
+ devm_kfree(ctrl_blk->mdev->dev, port_mngr);
+ ctrl_blk->port_mngr = NULL;
+ dev_info(ctrl_blk->mdev->dev, "Exit port_mngr successfully\n");
+}
diff --git a/drivers/net/wwan/mediatek/mtk_port.h b/drivers/net/wwan/mediatek/mtk_port.h
new file mode 100644
index 000000000000..dd6d47092028
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_port.h
@@ -0,0 +1,171 @@
+/* SPDX-License-Identifier: BSD-3-Clause-Clear
+ *
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#ifndef __MTK_PORT_H__
+#define __MTK_PORT_H__
+
+#include <linux/bits.h>
+#include <linux/mutex.h>
+#include <linux/radix-tree.h>
+#include <linux/skbuff.h>
+#include <linux/types.h>
+
+#include "mtk_ctrl_plane.h"
+#include "mtk_dev.h"
+
+#define MTK_PEER_ID_MASK (0xF000)
+#define MTK_PEER_ID_SHIFT (12)
+#define MTK_PEER_ID(ch) (((ch) & MTK_PEER_ID_MASK) >> MTK_PEER_ID_SHIFT)
+#define MTK_PEER_ID_SAP (0x1)
+#define MTK_PEER_ID_MD (0x2)
+#define MTK_CH_ID_MASK (0x0FFF)
+#define MTK_CH_ID(ch) ((ch) & MTK_CH_ID_MASK)
+#define MTK_DFLT_MAX_DEV_CNT (10)
+#define MTK_DFLT_PORT_NAME_LEN (20)
+
+#define MTK_PORT_TBL_TYPE(ch) (MTK_PEER_ID(ch) - 1)
+
+#define MTK_CCCI_H_ELEN (128)
+
+#define MTK_HDR_FLD_AST ((u32)BIT(31))
+#define MTK_HDR_FLD_SEQ GENMASK(30, 16)
+#define MTK_HDR_FLD_CHN GENMASK(15, 0)
+
+#define MTK_INFO_FLD_EN ((u16)BIT(15))
+#define MTK_INFO_FLD_CHID GENMASK(14, 0)
+
+enum mtk_port_status {
+ PORT_S_DFLT = 0,
+ PORT_S_ENABLE,
+ PORT_S_OPEN,
+ PORT_S_RDWR,
+ PORT_S_FLUSH,
+ PORT_S_ON_STALE_LIST,
+};
+
+enum mtk_ccci_ch {
+ CCCI_SAP_CONTROL_RX = 0x1000,
+ CCCI_SAP_CONTROL_TX = 0x1001,
+ CCCI_CONTROL_RX = 0x2000,
+ CCCI_CONTROL_TX = 0x2001,
+};
+
+enum mtk_port_flag {
+ PORT_F_DFLT = 0,
+ PORT_F_BLOCKING = BIT(1),
+ PORT_F_ALLOW_DROP = BIT(2),
+};
+
+enum mtk_port_tbl {
+ PORT_TBL_SAP,
+ PORT_TBL_MD,
+ PORT_TBL_MAX
+};
+
+enum mtk_port_type {
+ PORT_TYPE_INTERNAL,
+ PORT_TYPE_MAX
+};
+
+struct mtk_internal_port {
+ void *arg;
+ int (*recv_cb)(void *arg, struct sk_buff *skb);
+};
+
+union mtk_port_priv {
+ struct mtk_internal_port i_priv;
+};
+
+struct mtk_port_cfg {
+ enum mtk_ccci_ch tx_ch;
+ enum mtk_ccci_ch rx_ch;
+ unsigned char vq_id;
+ enum mtk_port_type type;
+ char name[MTK_DFLT_PORT_NAME_LEN];
+ unsigned char flags;
+};
+
+struct mtk_port {
+ struct mtk_port_cfg info;
+ struct kref kref;
+ bool enable;
+ unsigned long status;
+ unsigned int minor;
+ unsigned short tx_seq;
+ unsigned short rx_seq;
+ unsigned int tx_mtu;
+ unsigned int rx_mtu;
+ struct sk_buff_head rx_skb_list;
+ unsigned int rx_data_len;
+ unsigned int rx_buf_size;
+ wait_queue_head_t trb_wq;
+ wait_queue_head_t rx_wq;
+ /* Use write_lock to lock user's write and disable thread */
+ struct mutex write_lock;
+ /* Used to lock user's read thread */
+ struct mutex read_buf_lock;
+ struct list_head stale_entry;
+ char dev_str[MTK_DEV_STR_LEN];
+ struct mtk_port_mngr *port_mngr;
+ union mtk_port_priv priv;
+};
+
+struct mtk_vq_info {
+ int tx_mtu;
+ int rx_mtu;
+ unsigned int port_cnt;
+ bool color;
+};
+
+struct mtk_port_mngr {
+ struct mtk_ctrl_blk *ctrl_blk;
+ struct radix_tree_root port_tbl[PORT_TBL_MAX];
+ struct mtk_vq_info vq_info[VQ_NUM];
+ struct kobject *port_attr_kobj;
+ int dev_id;
+};
+
+struct mtk_stale_list {
+ struct list_head entry;
+ struct list_head ports;
+ char dev_str[MTK_DEV_STR_LEN];
+ int dev_id;
+};
+
+struct mtk_port_info {
+ __le16 channel;
+ __le16 reserved;
+} __packed;
+
+struct mtk_port_enum_msg {
+ __le32 head_pattern;
+ __le16 port_cnt;
+ __le16 version;
+ __le32 tail_pattern;
+ u8 data[];
+} __packed;
+
+struct mtk_ccci_header {
+ __le32 packet_header;
+ __le32 packet_len;
+ __le32 status;
+ __le32 ex_msg;
+};
+
+extern const struct port_ops *ports_ops[PORT_TYPE_MAX];
+
+void mtk_port_release(struct kref *port_kref);
+struct mtk_port *mtk_port_search_by_name(struct mtk_port_mngr *port_mngr, char *name);
+void mtk_port_stale_list_grp_cleanup(void);
+int mtk_port_add_header(struct sk_buff *skb);
+struct mtk_ccci_header *mtk_port_strip_header(struct sk_buff *skb);
+int mtk_port_send_data(struct mtk_port *port, void *data);
+int mtk_port_vq_enable(struct mtk_port *port);
+int mtk_port_vq_disable(struct mtk_port *port);
+int mtk_port_mngr_vq_status_check(struct sk_buff *skb);
+int mtk_port_mngr_init(struct mtk_ctrl_blk *ctrl_blk);
+void mtk_port_mngr_exit(struct mtk_ctrl_blk *ctrl_blk);
+
+#endif /* __MTK_PORT_H__ */
diff --git a/drivers/net/wwan/mediatek/mtk_port_io.c b/drivers/net/wwan/mediatek/mtk_port_io.c
new file mode 100644
index 000000000000..2ddd131dfe16
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_port_io.c
@@ -0,0 +1,253 @@
+// SPDX-License-Identifier: BSD-3-Clause-Clear
+/*
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#include "mtk_port_io.h"
+
+#define MTK_DFLT_READ_TIMEOUT (1 * HZ)
+
+static int mtk_port_get_locked(struct mtk_port *port)
+{
+ int ret = 0;
+
+ mutex_lock(&port_mngr_grp_mtx);
+ if (!port) {
+ mutex_unlock(&port_mngr_grp_mtx);
+ pr_err("[TMI] Port does not exist\n");
+ return -ENODEV;
+ }
+ kref_get(&port->kref);
+ mutex_unlock(&port_mngr_grp_mtx);
+
+ return ret;
+}
+
+static void mtk_port_put_locked(struct mtk_port *port)
+{
+ mutex_lock(&port_mngr_grp_mtx);
+ kref_put(&port->kref, mtk_port_release);
+ mutex_unlock(&port_mngr_grp_mtx);
+}
+
+static void mtk_port_struct_init(struct mtk_port *port)
+{
+ port->tx_seq = 0;
+ port->rx_seq = -1;
+ clear_bit(PORT_S_ENABLE, &port->status);
+ kref_init(&port->kref);
+ skb_queue_head_init(&port->rx_skb_list);
+ port->rx_buf_size = MTK_RX_BUF_SIZE;
+ init_waitqueue_head(&port->trb_wq);
+ init_waitqueue_head(&port->rx_wq);
+ mutex_init(&port->read_buf_lock);
+}
+
+static int mtk_port_internal_init(struct mtk_port *port)
+{
+ mtk_port_struct_init(port);
+ port->enable = false;
+
+ return 0;
+}
+
+static int mtk_port_internal_exit(struct mtk_port *port)
+{
+ if (test_bit(PORT_S_ENABLE, &port->status))
+ ports_ops[port->info.type]->disable(port);
+
+ return 0;
+}
+
+static int mtk_port_reset(struct mtk_port *port)
+{
+ port->tx_seq = 0;
+ port->rx_seq = -1;
+
+ return 0;
+}
+
+static int mtk_port_internal_enable(struct mtk_port *port)
+{
+ int ret;
+
+ if (test_bit(PORT_S_ENABLE, &port->status)) {
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Skip to enable port( %s )\n", port->info.name);
+ return 0;
+ }
+
+ ret = mtk_port_vq_enable(port);
+ if (ret && ret != -EBUSY)
+ return ret;
+
+ set_bit(PORT_S_RDWR, &port->status);
+ set_bit(PORT_S_ENABLE, &port->status);
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Port(%s) enable is complete\n", port->info.name);
+
+ return 0;
+}
+
+static int mtk_port_internal_disable(struct mtk_port *port)
+{
+ if (!test_and_clear_bit(PORT_S_ENABLE, &port->status)) {
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Skip to disable port(%s)\n", port->info.name);
+ return 0;
+ }
+
+ clear_bit(PORT_S_RDWR, &port->status);
+ mtk_port_vq_disable(port);
+
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev,
+ "Port(%s) disable is complete\n", port->info.name);
+
+ return 0;
+}
+
+static int mtk_port_internal_recv(struct mtk_port *port, struct sk_buff *skb)
+{
+ struct mtk_internal_port *priv;
+ int ret = -ENXIO;
+
+ if (!test_bit(PORT_S_OPEN, &port->status)) {
+ dev_warn_ratelimited(port->port_mngr->ctrl_blk->mdev->dev,
+ "Unabled to recv: (%s) not opened\n", port->info.name);
+ goto drop_data;
+ }
+
+ priv = &port->priv.i_priv;
+ if (!priv->recv_cb || !priv->arg) {
+ dev_warn_ratelimited(port->port_mngr->ctrl_blk->mdev->dev,
+ "Invalid (%s) recv_cb, drop packet\n", port->info.name);
+ goto drop_data;
+ }
+
+ ret = priv->recv_cb(priv->arg, skb);
+ return ret;
+
+drop_data:
+ dev_kfree_skb_any(skb);
+ return ret;
+}
+
+static int mtk_port_common_open(struct mtk_port *port)
+{
+ int ret = 0;
+
+ if (!test_bit(PORT_S_ENABLE, &port->status)) {
+ pr_err("[TMI] Failed to open: (%s) is disabled\n", port->info.name);
+ ret = -ENODEV;
+ goto end;
+ }
+
+ if (test_bit(PORT_S_OPEN, &port->status)) {
+ dev_warn(port->port_mngr->ctrl_blk->mdev->dev,
+ "Unabled to open port(%s) twice\n", port->info.name);
+ ret = -EBUSY;
+ goto end;
+ }
+
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev, "Open port %s\n", port->info.name);
+ skb_queue_purge(&port->rx_skb_list);
+ set_bit(PORT_S_OPEN, &port->status);
+
+end:
+ return ret;
+}
+
+static void mtk_port_common_close(struct mtk_port *port)
+{
+ dev_info(port->port_mngr->ctrl_blk->mdev->dev, "Close port %s\n", port->info.name);
+
+ clear_bit(PORT_S_OPEN, &port->status);
+
+ skb_queue_purge(&port->rx_skb_list);
+}
+
+void *mtk_port_internal_open(struct mtk_md_dev *mdev, char *name, int flag)
+{
+ struct mtk_port_mngr *port_mngr;
+ struct mtk_ctrl_blk *ctrl_blk;
+ struct mtk_port *port;
+ int ret;
+
+ ctrl_blk = mdev->ctrl_blk;
+ port_mngr = ctrl_blk->port_mngr;
+
+ port = mtk_port_search_by_name(port_mngr, name);
+ ret = mtk_port_get_locked(port);
+ if (ret)
+ goto end;
+
+ ret = mtk_port_common_open(port);
+ if (ret) {
+ mtk_port_put_locked(port);
+ goto end;
+ }
+
+ if (flag & O_NONBLOCK)
+ port->info.flags &= ~PORT_F_BLOCKING;
+ else
+ port->info.flags |= PORT_F_BLOCKING;
+end:
+ return port;
+}
+
+int mtk_port_internal_close(void *i_port)
+{
+ struct mtk_port *port = i_port;
+ int ret = 0;
+
+ if (!port) {
+ ret = -EINVAL;
+ goto end;
+ }
+
+ if (!test_bit(PORT_S_OPEN, &port->status)) {
+ pr_err("[TMI] Port(%s) has been closed\n", port->info.name);
+ ret = -EBADF;
+ goto end;
+ }
+
+ mtk_port_common_close(port);
+ mtk_port_put_locked(port);
+end:
+ return ret;
+}
+
+int mtk_port_internal_write(void *i_port, struct sk_buff *skb)
+{
+ struct mtk_port *port = i_port;
+
+ if (!port)
+ return -EINVAL;
+
+ return mtk_port_send_data(port, skb);
+}
+
+void mtk_port_internal_recv_register(void *i_port,
+ int (*cb)(void *priv, struct sk_buff *skb),
+ void *arg)
+{
+ struct mtk_port *port = i_port;
+ struct mtk_internal_port *priv;
+
+ priv = &port->priv.i_priv;
+ priv->arg = arg;
+ priv->recv_cb = cb;
+}
+
+static const struct port_ops port_internal_ops = {
+ .init = mtk_port_internal_init,
+ .exit = mtk_port_internal_exit,
+ .reset = mtk_port_reset,
+ .enable = mtk_port_internal_enable,
+ .disable = mtk_port_internal_disable,
+ .recv = mtk_port_internal_recv,
+};
+
+const struct port_ops *ports_ops[PORT_TYPE_MAX] = {
+ &port_internal_ops,
+};
diff --git a/drivers/net/wwan/mediatek/mtk_port_io.h b/drivers/net/wwan/mediatek/mtk_port_io.h
new file mode 100644
index 000000000000..30e1d4149881
--- /dev/null
+++ b/drivers/net/wwan/mediatek/mtk_port_io.h
@@ -0,0 +1,34 @@
+/* SPDX-License-Identifier: BSD-3-Clause-Clear
+ *
+ * Copyright (c) 2022, MediaTek Inc.
+ */
+
+#ifndef __MTK_PORT_IO_H__
+#define __MTK_PORT_IO_H__
+
+#include <linux/netdevice.h>
+#include <linux/skbuff.h>
+
+#include "mtk_port.h"
+
+#define MTK_RX_BUF_SIZE (1024 * 1024)
+
+extern struct mutex port_mngr_grp_mtx;
+
+struct port_ops {
+ int (*init)(struct mtk_port *port);
+ int (*exit)(struct mtk_port *port);
+ int (*reset)(struct mtk_port *port);
+ int (*enable)(struct mtk_port *port);
+ int (*disable)(struct mtk_port *port);
+ int (*recv)(struct mtk_port *port, struct sk_buff *skb);
+};
+
+void *mtk_port_internal_open(struct mtk_md_dev *mdev, char *name, int flag);
+int mtk_port_internal_close(void *i_port);
+int mtk_port_internal_write(void *i_port, struct sk_buff *skb);
+void mtk_port_internal_recv_register(void *i_port,
+ int (*cb)(void *priv, struct sk_buff *skb),
+ void *arg);
+
+#endif /* __MTK_PORT_IO_H__ */
diff --git a/drivers/net/wwan/mediatek/pcie/mtk_pci.c b/drivers/net/wwan/mediatek/pcie/mtk_pci.c
index b3de9634a62c..d1cf4bf10f6a 100644
--- a/drivers/net/wwan/mediatek/pcie/mtk_pci.c
+++ b/drivers/net/wwan/mediatek/pcie/mtk_pci.c
@@ -13,6 +13,7 @@
#include <linux/kernel.h>
#include <linux/module.h>
+#include "../mtk_port_io.h"
#include "mtk_pci.h"
#include "mtk_reg.h"
@@ -943,6 +944,7 @@ module_init(mtk_drv_init);
static void __exit mtk_drv_exit(void)
{
pci_unregister_driver(&mtk_pci_drv);
+ mtk_port_stale_list_grp_cleanup();
}
module_exit(mtk_drv_exit);
--
2.32.0© 2016 - 2024 Red Hat, Inc.